├── 3D.jpg
├── Estimate.m
├── README.md
├── TrackTarget.asv
└── TrackTarget.m


/3D.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gadhane/Kalman-Filter-for-Fusing-Radar-Sensor-Measurements-in-3D/da8768872abee445cdb5b7c16dd3dc2a9e6cf918/3D.jpg


--------------------------------------------------------------------------------
/Estimate.m:
--------------------------------------------------------------------------------
 1 | function [Range,Azimuth,Elevation] =Estimate(Loc_estimate_x,Loc_estimate_y,Loc_estimate_z)
 2 | Rad_pos=[0 0 0];
 3 | target_x=Loc_estimate_x;%Q_estimate(1);
 4 | target_y=Loc_estimate_y;%Q_estimate(2);
 5 | target_z=Loc_estimate_z;%Q_estimate(3);
 6 | 
 7 | %Compute Range
 8 | Range=sqrt((Rad_pos(1)-target_x)^2+(Rad_pos(2)-target_y)^2+(Rad_pos(3)-target_z)^2);
 9 | %Compute Azimuth
10 | Azimuth = atan((target_y - Rad_pos(2))/(target_x - Rad_pos(1)))*57.3;
11 | 
12 | %Azimuth=atan((target_x-Rad_pos(1))/(target_y-Rad_pos(2)))*57.3;
13 | 
14 | %Compute Elevation
15 | Elevation=atan((target_z-Rad_pos(3))/sqrt((target_y-Rad_pos(2)).^2+(target_x-Rad_pos(1))^.2))*57.3;
16 | if Range<=120
17 |     fprintf('Range is %f\n',Range);
18 |     fprintf('Azimuth is %f\n',Azimuth);
19 |     fprintf('Elevation is %f\n',Elevation);
20 | else 
21 |     fprintf('No more lock!');
22 | end


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Kalman-Filter-for-Fusing-Radar-Sensor-Measurements-in-3D
2 | State estimation for target tracking (Single Sensor, Single Target) with nonlinear kalman filter
3 | This is an extended kalman filter implementation in matlab for fusing radar sensor measurement in 3D. The sensor measures the three-dimensional target including the range (distance) <i>r</i>, azimuth angle (turning angle) \Theta,
4 | and elevation angle (elevation angle) \Phi . 
5 |  
6 | Range <i>r</i> denotes the measured distance of the target from the center of the tracker, while the azimuth angle or swivel angle \Theta, shows the direction of motion of the tracker rotation in the direction of the x-axis and y-axis, and the elevation angle \Phi indicates the direction of motion of trackers' elevation in the direction of <i>x, y,</i> to<i> z </i>axis. 
7 | 
8 | <H1>This file is not yet completed. Equations, code explanation and pictures will be uploaded soon.</h1>
9 | 


--------------------------------------------------------------------------------
/TrackTarget.asv:
--------------------------------------------------------------------------------
 1 | close all;
 2 | clear all;
 3 | dt=0.1;
 4 | u=2.55;
 5 | duration=10;
 6 | CM_IDX=[];
 7 | Q=[2; 2; 2;1;1; 0];
 8 | Q_estimate=Q;
 9 | Accel_Noise_Mag=0.05;
10 | tkn_x=1;%measurement noise in x 
11 | tkn_y=1;%measurement noise in y
12 | tkn_z=1;%measurement noise in z
13 | tkn_vx=1;%measurement noise of velocity in x
14 | tkn_vy=2;%measurement noise of velocity in y
15 | tkn_vy=1;%Measurement noise of Velocity in Z
16 | measurement_noise_mag=10;
17 | 
18 | Ez=[tkn_x^2 0 0 0 0 0;0 tkn_y^2 0 0 0 0;0 0 tkn_z^2 0 0 0;0 0 0 tkn_vx^2 0 0;0 0 0 0 tkn_vy^2 0;0 0 0 0 0 tkn_vz^2];
19 | Ex=Accel_Noise_Mag^2*[dt^4/4 0 0 dt^3 0 0;0 dt^4/4 0 0 dt^3/2 0;0 0 dt^4/4 0 0 dt^2;dt^3/2 0 0 dt^2 0 0;0 dt^3/2 0 0 dt^2 0;0 0 dt^3/2 0 0 dt^2];% process error covariance
20 | P=Ex;
21 | 
22 | A=[1 0 0 dt 0 0;0 1 0 0 dt 0;0 0 1 0 0 dt;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];% transition matrix
23 | B=[dt^2/2;dt^2/2;0;dt;dt,0];%control matrix in this case accelaration
24 | C=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];%measurement matrix
25 | %Initial variables
26 | Q_Loc=[];
27 | Q_Loc_x=[];
28 | Q_Loc_y=[];
29 | Q_Loc_z=[];
30 | Vel_measured=[];
31 | Q_Measure=[];
32 | Q_Loc_MeasureX=[];
33 | Q_Loc_MeasureY=[];
34 | Q_Loc_MeasureZ=[];
35 | 
36 | figure(1);clf
37 | for t=0:dt:duration
38 |     targetAccel_noise=Accel_Noise_Mag*[(dt^2/2)*randn;(dt^2/2)*randn;0;dt*randn;dt*randn,0];
39 |     Q=A*Q+B*u+targetAccel_noise;
40 |     measurement_noise=measurement_noise_mag*randn;
41 |     Measurement=C*Q+measurement_noise;
42 |     Q_Loc=[Q_Loc Q(1:3)];
43 |     Q_Loc_x=[Q_Loc_x;Q(1)];
44 |     Q_Loc_y=[Q_Loc_y;Q(2)];
45 |     Q_Loc_z=[Q_Loc_z;Q(3)];
46 |     Q_Measure=[Q_Measure Measurement];
47 |     Q_Loc_MeasureX=[Q_Loc_MeasureX;Measurement(1)];
48 |     Q_Loc_MeasureY=[Q_Loc_MeasureY;Measurement(2)];
49 |     Q_Loc_MeasureZ=[Q_Loc_MeasureZ;Measurement(3)];
50 |     Vel_measured=[Vel_measured Measurement(4:6)];
51 |     
52 |     plot3(Q_Loc_MeasureX,Q_Loc_MeasureY,Q_Loc_MeasureZ,'ro');
53 |     plot3(Q_Loc_x,Q_Loc_y,Q_Loc_z,'b*');
54 |     hold on
55 |     grid on
56 |     %pause
57 | end


--------------------------------------------------------------------------------
/TrackTarget.m:
--------------------------------------------------------------------------------
  1 | close all;
  2 | clear all;
  3 | dt=0.1;
  4 | u=2.55;
  5 | duration=10;
  6 | CM_IDX=[];
  7 | Q=[2; 2; 2;1;1; 0];
  8 | Q_estimate=Q;
  9 | Accel_Noise_Mag=0.05;
 10 | tkn_x=1;%measurement noise in x 
 11 | tkn_y=1;%measurement noise in y
 12 | tkn_z=1;%measurement noise in z
 13 | tkn_vx=1;%measurement noise of velocity in x
 14 | tkn_vy=2;%measurement noise of velocity in y
 15 | tkn_vz=1;%Measurement noise of Velocity in Z
 16 | measurement_noise_mag=10;
 17 | 
 18 | Ez=[tkn_x^2 0 0 0 0 0;0 tkn_y^2 0 0 0 0;0 0 tkn_z^2 0 0 0;0 0 0 tkn_vx^2 0 0;0 0 0 0 tkn_vy^2 0;0 0 0 0 0 tkn_vz^2];
 19 | Ex=Accel_Noise_Mag^2*[dt^4/4 0 0 dt^3 0 0;0 dt^4/4 0 0 dt^3/2 0;0 0 dt^4/4 0 0 dt^2;dt^3/2 0 0 dt^2 0 0;0 dt^3/2 0 0 dt^2 0;0 0 dt^3/2 0 0 dt^2];% process error covariance
 20 | P=Ex;
 21 | 
 22 | A=[1 0 0 dt 0 0;0 1 0 0 dt 0;0 0 1 0 0 dt;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];% transition matrix
 23 | B=[dt^2/2;dt^2/2;0;dt;dt;0];%control matrix in this case accelaration
 24 | C=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];%measurement matrix
 25 | %Initial variables
 26 | Q_Loc=[];
 27 | Q_Loc_x=[];
 28 | Q_Loc_y=[];
 29 | Q_Loc_z=[];
 30 | Vel_measured=[];
 31 | Q_Measure=[];
 32 | Q_Loc_MeasureX=[];
 33 | Q_Loc_MeasureY=[];
 34 | Q_Loc_MeasureZ=[];
 35 | 
 36 | figure(1);clf
 37 | for t=0:dt:duration
 38 |     targetAccel_noise=Accel_Noise_Mag*[(dt^2/2)*randn;(dt^2/2)*randn;0;dt*randn;dt*randn;0];
 39 |     Q=A*Q+B*u+targetAccel_noise;
 40 |     measurement_noise=measurement_noise_mag*randn;
 41 |     Measurement=C*Q+measurement_noise;
 42 |     Q_Loc=[Q_Loc Q(1:3)];
 43 |     Q_Loc_x=[Q_Loc_x;Q(1)];
 44 |     Q_Loc_y=[Q_Loc_y;Q(2)];
 45 |     Q_Loc_z=[Q_Loc_z;Q(3)];
 46 |     Q_Measure=[Q_Measure Measurement];
 47 |     Q_Loc_MeasureX=[Q_Loc_MeasureX;Measurement(1)];
 48 |     Q_Loc_MeasureY=[Q_Loc_MeasureY;Measurement(2)];
 49 |     Q_Loc_MeasureZ=[Q_Loc_MeasureZ;Measurement(3)];
 50 |     Vel_measured=[Vel_measured Measurement(4:6)];
 51 |     
 52 |     plot3(Q_Loc_MeasureX,Q_Loc_MeasureY,Q_Loc_MeasureZ,'ro');
 53 |     plot3(Q_Loc_x,Q_Loc_y,Q_Loc_z,'b*');
 54 |     hold on
 55 |     grid on
 56 |     %pause
 57 | end
 58 | 
 59 | %Estimated variables
 60 | Q_Loc_estimate=[];
 61 | Q_Loc_estimateX=[];
 62 | Q_Loc_estimateY=[];
 63 | Q_Loc_estimateZ=[];
 64 | Vel_estimate=[];
 65 | Q=[0; 0; 0;0; 0;0];
 66 | P_estimate=P;
 67 | Predic_state=[];
 68 | Predic_var=[];
 69 | 
 70 | for t=1:length(Q_Loc)
 71 |     Q_estimate=A*Q_estimate+B*u;
 72 |     Predic_state=[Predic_state Q_estimate];
 73 |     P=A*P*A'+Ex;
 74 |     Predic_var=[Predic_var P];
 75 |     K=P*C'*inv(C*P*C'+Ez);
 76 |     if ~isnan(Q_Measure(:,t))
 77 |      Q_estimate=Q_estimate+K*(Q_Measure(:,t)-C*Q_estimate);
 78 |     end
 79 |     P=(eye(6)-K*C)*P;
 80 |     Q_Loc_estimate=[Q_Loc_estimate Q_estimate(1:3)];
 81 |     Q_Loc_estimateX=[Q_Loc_estimateX;Q_estimate(1)];
 82 |     Q_Loc_estimateY=[Q_Loc_estimateY;Q_estimate(2)];
 83 |     Q_Loc_estimateZ=[Q_Loc_estimateZ;Q_estimate(3)];
 84 |     Vel_estimate=[Vel_estimate Q_estimate(4:6)];
 85 |     [r,a,e] = Estimate(Q_estimate(1),Q_estimate(2),Q_estimate(3));
 86 |         %if (t>1&&t<length(Q_Loc))
 87 |         %r(t)=r;
 88 |         %v=(r(t)-r(t-1))./dt;
 89 |         %end
 90 |         %
 91 |         %lqe();kalman filter estimator built in matlab function
 92 |         %lqr built in function used to calculate kalman gain
 93 |     hold on
 94 |     grid on
 95 | end
 96 | figure(2);clf
 97 | tt=0:dt:duration;
 98 | plot(tt,Q_Loc_y,'-r.',tt,Q_Loc_MeasureY,'-k.',tt,Q_Loc_estimateY,'-g.');
 99 | figure(3);clf
100 | plot3(Q_Loc_x,Q_Loc_y,Q_Loc_z,'g+');
101 | hold on
102 | plot3(Q_Loc_MeasureX,Q_Loc_MeasureY,Q_Loc_MeasureZ,'b*');
103 | 
104 | hold on
105 | plot3(Q_Loc_estimateX,Q_Loc_estimateY,Q_Loc_estimateZ,'ro');
106 | 
107 | hold on
108 | legend('Actual location','Mesaured location','Estimated location');
109 | grid on


--------------------------------------------------------------------------------