├── CTRV_UKF.jpg ├── CTRV_UKF.m ├── LKF.m ├── LKF.png ├── README.md ├── UKF.m ├── UKF.png ├── main.cpp └── trueTarget.mat /CTRV_UKF.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jeffrey-antoine/RadarDataProcessing/05b9b86f59244b776c418b50fbff5550371c4a2b/CTRV_UKF.jpg -------------------------------------------------------------------------------- /CTRV_UKF.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jeffrey-antoine/RadarDataProcessing/05b9b86f59244b776c418b50fbff5550371c4a2b/CTRV_UKF.m -------------------------------------------------------------------------------- /LKF.m: -------------------------------------------------------------------------------- 1 | % Linear Kalman Filter Demo 2 | % Jileil 2018-03-23 3 | clc;clear;close all; 4 | load trueTarget.mat 5 | % All rights reserved 6 | %% generate noisyMeasurement variable 7 | % this section generates random noisy measurement variable 8 | % set the covariance matrix 9 | % Assume noise of X and Y are independent, so the non-diagnoal elements are zeros 10 | % Sigma = [SigmaX^2 0; 11 | % 0 SigmaY^2] 12 | sigma = 100; 13 | Sigma = [sigma^2 0;0 sigma^2]; 14 | % Initialize noisyMeasurement 15 | noisyMeasurement = zeros(size(trueTarget)); 16 | noisyMeasurement(1,:) = trueTarget(1,:); 17 | for i = 1:size(trueTarget,2) 18 | % Generate pseudo trueTarget 19 | %randn is the random normal ditribution generator 20 | % get cholesky decomposition A = LL', L is a upper triangle matrix in matlab 21 | niu = chol(Sigma)*randn(2,1); 22 | % Znoisy = Ztrue + Noisy 23 | % Generate Noise Measurement 2 24 | noisyMeasurement(2:3,i) = trueTarget(2:3,i) + niu; 25 | end 26 | Z = noisyMeasurement(2:3,:); 27 | clear Sigma sigma niu i; 28 | %% Kalman Fliter implementation 29 | h = figure; 30 | set(h,'position',[100,100,800,800]); 31 | 32 | % x0_: mean of x0, initialization of status vector 33 | x0_ = [1000 1000 0 0]'; 34 | % P0 : the initialization of covariance matrix 35 | P0 = [10000 0 0 0; 36 | 0 10000 0 0; 37 | 0 0 100 0; 38 | 0 0 0 100]; 39 | % x0 is normal distributed as N(x0;x0_,P0) 40 | %x0 = chol(P0)*randn(4,1) + x0_; 41 | %x0 = x0_; 42 | % Assume time interval T = 1 43 | T = 1; 44 | % ********************* CV Model ********************** 45 | % status transfer matrix in CV(constant-velocity) model as A or F: 46 | % [I2 T*I2 = [1 0 T 0 47 | % 02 I2 ] 0 1 0 T 48 | % 0 0 1 0 49 | % 0 0 0 1] 50 | A = [1 0 T 0; 51 | 0 1 0 T; 52 | 0 0 1 0; 53 | 0 0 0 1]; 54 | % noisy transfer matrix in CV(constant-velocity) model as B or T: 55 | % [0.5*T^2*I2 = [0.5T^2 0 56 | % T*I2] 0 0.5T^2 57 | % T 0 58 | % 0 T] 59 | B = [0.5*T^2 0; 60 | 0 0.5*T^2; 61 | T 0; 62 | 0 T;]; 63 | % process noise model 64 | % the dynamic property or the instability of system increases with Q; 65 | % when Q increases, the K gets bigger 66 | pnsigma = 1; 67 | processNoiseSigma = [pnsigma^2 0;0 pnsigma^2]; 68 | % this is Q 69 | % noiseVector = chol(eye(2))*randn(2,1); 70 | Q = B * processNoiseSigma * B';%[4*4] which is a constant under the Gaussian Assumption 71 | 72 | H = [1 0 0 0; 73 | 0 1 0 0]; 74 | % 75 | xEstimate = zeros(4,151); 76 | pEstimate = zeros(16,151); 77 | xPredict = zeros(4,151); 78 | zPredict = zeros(2,151); 79 | innovation = zeros(2,151); 80 | % initial_estimate= z0 + chol(p0_v)*rand(2,1) 81 | a0 = [Z(:,1);chol([100 0;0 100])*rand(2,1)]; 82 | xEstimate(:,1) = a0; 83 | pEstimate(:,1) = reshape(P0,16,1); 84 | % under real circumstance, we don't know the pdf of measurement noise 85 | sigma2 = 100; 86 | R = [sigma2^2 0;0 sigma2^2]; 87 | for k = 2:151 88 | xPredict(:,k) = A * xEstimate(:,k - 1); % 1-step-ahead vector of state forecasts 89 | pPredict = A * reshape(pEstimate(: , k - 1),4 , 4) * A.' + Q; %[4 4] % 1-step-ahead covariance 90 | % 1-step-ahead vector of observation forecasts 91 | % zPredict = E[H(k+1)*X(K+1)+W(K+1)|Z^k], due to the assumption that 92 | % E[W(K+1)] = 0, zPredict can be simplified as: 93 | zPredict(:,k) = H * xPredict(:,k); 94 | % 1-step-ahead estimated of observation covariance 95 | % R actually need to be calculated as Q 96 | Spredict = H * pPredict * H' + R; %[2 2] 97 | K = pPredict * H'/ Spredict; %[4 2] 98 | % observation innovation 99 | innovation(:,k) = Z(:,k) - zPredict(:,k); % innovation [2 1] 100 | xEstimate(:,k) = xPredict(:,k) + K * innovation(:,k); % [4 1] 101 | %pEstimate(:,k) = reshape(pPredict - K * Spredict * K',16 ,1); %[4 4] 102 | AA = eye(length(xEstimate(:,k))) - K * H; 103 | pEstimate_temp = AA * pPredict * AA.' + K * R * K.'; 104 | pEstimate(:,k) = reshape(pEstimate_temp,16,1); 105 | end 106 | hold on; 107 | plot(trueTarget(2,:),trueTarget(3,:),'b*-'); 108 | 109 | plot(Z(1,:),Z(2,:),'r>'); 110 | plot(xEstimate(1,:),xEstimate(2,:),'go-'); 111 | -------------------------------------------------------------------------------- /LKF.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jeffrey-antoine/RadarDataProcessing/05b9b86f59244b776c418b50fbff5550371c4a2b/LKF.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Radar 2 | Implementations of some basic algorithms in radar data processing 3 | ## Kalman Filter 4 | ### LKF 5 | ![image](https://github.com/jeffrey-antoine/RadarDataProcessing/blob/master/LKF.png) 6 | ### UKF 7 | ![image](https://github.com/jeffrey-antoine/RadarDataProcessing/blob/master/UKF.png) 8 | ### CTRV+UKF 9 | ![image](https://github.com/jeffrey-antoine/RadarDataProcessing/blob/master/CTRV_UKF.jpg) 10 | -------------------------------------------------------------------------------- /UKF.m: -------------------------------------------------------------------------------- 1 | % UKF demo 2 | % Jileil 2018-03-26 3 | clc;clear;close all; 4 | load trueTarget.mat; 5 | rng(7); 6 | %% UKF 7 | % generate noisy measurement in non-linear coordinate 8 | sigma_r = 100; 9 | sigma_theta = 5; 10 | % [ V_r(k) ~ N([0 ,[sigma_r^2 0 11 | % V_theta(k)] 0] 0 sigma_theta^2] 12 | r = sqrt(trueTarget(2,:).^2 + trueTarget(3,:).^2); 13 | theta = atan(trueTarget(3,:)./trueTarget(2,:))/pi*180; 14 | %z_rho_theta = 15 | Z_degree = [r;theta]; 16 | 17 | for i = 1:length(r) 18 | Z_degree(:,i) = Z_degree(:,i) + [100 0;0 5]*randn(2,1); 19 | end 20 | 21 | %% 22 | %****************Assume Only measurement equation is different*********** 23 | 24 | % x0_: mean of x0, initialization of status vector 25 | x0_ = [1000 1000 0 0]'; 26 | % P0 : the initialization of covariance matrix 27 | P0 = [10000 0 0 0; 28 | 0 10000 0 0; 29 | 0 0 100 0; 30 | 0 0 0 100]; 31 | 32 | % Assume time interval T = 1 33 | T = 1; 34 | % status transfer matrix in CV(constant-velocity) model as A or F: 35 | % [I2 T*I2 = [1 0 T 0 36 | % 02 I2 ] 0 1 0 T 37 | % 0 0 1 0 38 | % 0 0 0 1] 39 | A = [1 0 T 0; 40 | 0 1 0 T; 41 | 0 0 1 0; 42 | 0 0 0 1]; 43 | % noisy transfer matrix in CV(constant-velocity) model as B or T: 44 | % [0.5*T^2*I2 = [0.5T^2 0 45 | % T*I2] 0 0.5T^2 46 | % T 0 47 | % 0 T] 48 | B = [0.5*T^2 0; 49 | 0 0.5*T^2; 50 | T 0; 51 | 0 T;]; 52 | pnsigma = 1; 53 | processNoiseSigma = [pnsigma^2 0;0 pnsigma^2]; 54 | % this is Q 55 | % noiseVector = chol(eye(2))*randn(2,1); 56 | Q = B * processNoiseSigma * B';%[4*4] 57 | %*******************basic UKF***************** 58 | n = 4; 59 | L = 2 * n + 1; 60 | xPredict_linear_hat = zeros(4,151); 61 | xPredict_sumSigma_hat = zeros(4,151); 62 | xEstimate = zeros(4,151); 63 | % Initialize x0, p0 64 | xEstimate(1:2,1) = [1000,1000]; 65 | pEstimate = zeros(16,151); 66 | 67 | R = [100^2 0;0 5^2]; 68 | 69 | pPredict_sigmaPoint = zeros(L*16,151); % for each sigma Point, there is a P 70 | zPredict_sigmaPoint = zeros(2,L); % for each sigma point, there is a measurement predict 71 | xPredict_sigmaPoint = zeros(4,L); % 4, L 72 | delta_x = zeros(1,n); 73 | w0 = 1/9; 74 | wi = (1 - w0)/(2*n); % for this case, wi = w0 75 | %% 76 | for k = 2:151 77 | % estimation is the same as LKF 78 | pEstimate_reshape = reshape(pEstimate(:,k - 1),4,4); 79 | xPredict_linear_hat(:,k) = A * xEstimate(:,k - 1); % 1-step-ahead vector of state forecasts 80 | pPredict_linear = A * reshape(pEstimate(: , k - 1),4 , 4) * A.' + Q; %[4 4] % 1-step-ahead covariance 81 | % generate sigma point 82 | [u,s] = svd(pPredict_linear); 83 | xPredict_sigmaPoint(:,L) = xPredict_linear_hat(:,k); % x0 is placed at the end of array 84 | delta_x = sqrt(n/(1-w0)) * u * sqrt(s); % get the square root of pEstimate and remain the same dimension 85 | 86 | % 87 | for i = 1:n 88 | % use basic 89 | xPredict_sigmaPoint(:,i) = xPredict_linear_hat(:,k) + delta_x(:,i); 90 | xPredict_sigmaPoint(:,i + n) = xPredict_linear_hat(:,k) - delta_x(:,i); 91 | end 92 | 93 | % measurement_prediction 94 | zPredict_sigmaPoint(1,:) = sqrt(xPredict_sigmaPoint(1,:).^2 + xPredict_sigmaPoint(2,:).^2); 95 | zPredict_sigmaPoint(2,:) = atan(xPredict_sigmaPoint(2,:)./xPredict_sigmaPoint(1,:))/pi*180;% radius to degree 96 | % wi = w0 in this example 97 | zPredict = wi * sum(zPredict_sigmaPoint,2); 98 | zll = zPredict_sigmaPoint - repmat(zPredict,1,L); 99 | xll = xPredict_sigmaPoint - repmat(xPredict_linear_hat(:,k),1,L); 100 | P_zz = R + wi * (zll * zll'); 101 | P_xz = wi * (xll * zll'); 102 | % kalman gain 103 | K = P_xz / P_zz; 104 | % correct the state 105 | xEstimate(:,k) = xPredict_linear_hat(:,k) + K * (Z_degree(:,k) - zPredict); 106 | % correct the covariance 107 | pEstimate_temp = pPredict_linear - K * P_zz * K'; 108 | pEstimate(:,k) = reshape(pEstimate_temp,16,1); 109 | end 110 | 111 | x_rd(1,:) = sqrt(xEstimate(1,:).^2 + xEstimate(2,:).^2); 112 | x_rd(2,:) = atan(xEstimate(2,:)./xEstimate(1,:)); 113 | trueTarget_rd(1,:) = sqrt(trueTarget(2,:).^2 + trueTarget(3,:).^2); 114 | trueTarget_rd(2,:) = atan(trueTarget(3,:)./trueTarget(2,:)); 115 | Z_radius = [Z_degree(1,:);Z_degree(2,:)*pi/180]; 116 | h = figure; 117 | set(h,'position',[100 100 800 800]); 118 | 119 | polarplot(Z_radius(2,:),Z_radius(1,:),'r-o'); 120 | hold on; 121 | polarplot(x_rd(2,:),x_rd(1,:),'g-*'); 122 | polarplot(trueTarget_rd(2,:),trueTarget_rd(1,:),'b->'); 123 | thetalim([0 90]); 124 | rlim([1000 3500]); 125 | -------------------------------------------------------------------------------- /UKF.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jeffrey-antoine/RadarDataProcessing/05b9b86f59244b776c418b50fbff5550371c4a2b/UKF.png -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | using namespace Eigen; 6 | using namespace std; 7 | 8 | double generate_random(double sigma) 9 | { 10 | static default_random_engine e(time(0)); 11 | 12 | static normal_distribution n(0, sigma); 13 | 14 | return n(e); 15 | } 16 | int main() 17 | { 18 | 19 | Matrix trueTarget; 20 | trueTarget << 1000, 1017.67767, 1035.355339, 1053.033009, 1070.710678, 1088.388348, 1106.066017, 1123.743687, 1141.421356, 1159.099026, 1176.776695, 1194.454365, 1212.132034, 1229.809704, 1247.487373, 1265.165043, 1282.842712, 1300.520382, 1318.198052, 1335.875721, 1353.553391, 1371.23106, 1388.90873, 1406.586399, 1424.264069, 1441.941738, 1459.619408, 1477.297077, 1494.974747, 1512.652416, 1530.330086, 1548.007755, 1565.685425, 1583.363094, 1601.040764, 1618.718434, 1636.396103, 1654.073773, 1671.751442, 1689.429112, 1707.106781, 1724.784451, 1742.46212, 1760.13979, 1777.817459, 1795.495129, 1813.172798, 1830.850468, 1848.528137, 1866.205807, 1883.883476, 1902.469119, 1922.113975, 1942.740515, 1964.267335, 1986.60948, 2009.678775, 2033.384175, 2057.632128, 2082.326936, 2107.37114, 2132.665904, 2158.111399, 2183.607205, 2209.052701, 2234.347464, 2259.391669, 2284.086477, 2308.334429, 2332.03983, 2355.109124, 2377.451269, 2398.97809, 2419.60463, 2439.249486, 2457.835128, 2475.288208, 2491.539846, 2506.525905, 2520.18724, 2532.469939, 2543.325525, 2552.711157, 2560.589794, 2566.930343, 2571.70778, 2574.903252, 2576.504147, 2576.504147, 2574.903252, 2571.70778, 2566.930343, 2560.589794, 2552.711157, 2543.325525, 2532.469939, 2520.18724, 2506.525905, 2491.539846, 2475.288208, 2457.835128, 2440.72145, 2423.607773, 2406.494095, 2389.380417, 2372.26674, 2355.153062, 2338.039384, 2320.925707, 2303.812029, 2286.698352, 2269.584674, 2252.470996, 2235.357319, 2218.243641, 2201.129963, 2184.016286, 2166.902608, 2149.78893, 2132.675253, 2115.561575, 2098.447897, 2081.33422, 2064.220542, 2047.106864, 2029.993187, 2012.879509, 1995.765832, 1978.652154, 1961.538476, 1944.424799, 1927.311121, 1910.197443, 1893.083766, 1875.970088, 1858.85641, 1841.742733, 1824.629055, 1807.515377, 1790.4017, 1773.288022, 1756.174344, 1739.060667, 1721.946989, 1704.833312, 1687.719634, 1670.605956, 1653.492279, 1636.378601, 1619.264923, 1602.151246, 1000, 1017.67767, 1035.355339, 1053.033009, 1070.710678, 1088.388348, 1106.066017, 1123.743687, 1141.421356, 1159.099026, 1176.776695, 1194.454365, 1212.132034, 1229.809704, 1247.487373, 1265.165043, 1282.842712, 1300.520382, 1318.198052, 1335.875721, 1353.553391, 1371.23106, 1388.90873, 1406.586399, 1424.264069, 1441.941738, 1459.619408, 1477.297077, 1494.974747, 1512.652416, 1530.330086, 1548.007755, 1565.685425, 1583.363094, 1601.040764, 1618.718434, 1636.396103, 1654.073773, 1671.751442, 1689.429112, 1707.106781, 1724.784451, 1742.46212, 1760.13979, 1777.817459, 1795.495129, 1813.172798, 1830.850468, 1848.528137, 1866.205807, 1883.883476, 1901.336556, 1917.588195, 1932.574253, 1946.235589, 1958.518287, 1969.373873, 1978.759505, 1986.638142, 1992.978691, 1997.756129, 2000.951601, 2002.552496, 2002.552496, 2000.951601, 1997.756129, 1992.978691, 1986.638142, 1978.759505, 1969.373873, 1958.518287, 1946.235589, 1932.574253, 1917.588195, 1901.336556, 1883.883476, 1865.297834, 1845.652978, 1825.026438, 1803.499617, 1781.157473, 1758.088178, 1734.382777, 1710.134825, 1685.440017, 1660.395813, 1635.101049, 1609.655554, 1584.159748, 1558.714252, 1533.419489, 1508.375284, 1483.680476, 1459.432524, 1435.727123, 1412.657829, 1390.315684, 1368.788863, 1348.162323, 1328.517467, 1309.931825, 1291.707609, 1273.483394, 1255.259178, 1237.034962, 1218.810747, 1200.586531, 1182.362315, 1164.138099, 1145.913884, 1127.689668, 1109.465452, 1091.241237, 1073.017021, 1054.792805, 1036.56859, 1018.344374, 1000.120158, 981.8959426, 963.6717269, 945.4475112, 927.2232955, 908.9990799, 890.7748642, 872.5506485, 854.3264328, 836.1022171, 817.8780014, 799.6537857, 781.4295701, 763.2053544, 744.9811387, 726.756923, 708.5327073, 690.3084916, 672.0842759, 653.8600603, 635.6358446, 617.4116289, 599.1874132, 580.9631975, 562.7389818, 544.5147661, 526.2905505, 508.0663348, 489.8421191, 471.6179034, 453.3936877, 435.169472, 416.9452563, 398.7210407; 21 | // generate measurement 22 | double sigma = 10; // set noise variance 23 | 24 | 25 | Array noise; 26 | for (int i = 0; i < 151*2; i++) 27 | { 28 | noise(i) = generate_random(sigma); 29 | } 30 | 31 | 32 | //cout << noise << endl; 33 | Matrix measure = trueTarget + noise.matrix(); 34 | cout << measure << endl; 35 | 36 | double T = 1; 37 | 38 | Matrix4d A; 39 | A << 1, 0, T, 0, 40 | 0, 1, 0, T, 41 | 0, 0, 1, 0, 42 | 0, 0, 0, 1; 43 | Matrix B; 44 | B << 0.5*T*T, 0, 45 | 0, 0.5*T*T, 46 | T,0, 47 | 0, T; 48 | cout << "B=" << endl << B << endl; 49 | MatrixH; 50 | H << 1, 0, 0, 0, 0, 1, 0, 0; 51 | cout << "H=" << endl << H << endl; 52 | Vector4d xk_1; 53 | Matrix4d Pk_1; 54 | xk_1 << measure(0,0), measure(0,1), 0, 0; 55 | Pk_1 << sigma * sigma, 0, 0, 0, 56 | 0, sigma*sigma, 0, 0, 57 | 0, 0, sigma, 0, 58 | 0, 0, 0, sigma; 59 | Matrix2d R; 60 | R< K; 63 | Matrix4d EMatrix; 64 | EMatrix.setIdentity(); 65 | 66 | cout<<"EMatrix = " << endl << EMatrix << endl; 67 | Matrix2d Qpro; 68 | Qpro.setIdentity(); 69 | Matrix4d Q = B * Qpro * B.transpose(); 70 | Matrix X_trace; 71 | 72 | for (int i = 1; i < 150; i++) 73 | { 74 | Vector4d x_pri = A * xk_1; 75 | Matrix4d P_pri = A * Pk_1*A.transpose()+Q; 76 | 77 | Matrix temp = P_pri * H.transpose(); 78 | MatrixXd temp2 = H * P_pri*H.transpose() + R; 79 | K = temp * temp2.inverse(); 80 | Vector2d measure_temp; 81 | measure_temp << measure(0, i), measure(1, i); 82 | 83 | xk_1 = x_pri + K * (measure_temp - H * x_pri); 84 | X_trace.col(i) = xk_1; 85 | 86 | Pk_1 = (EMatrix - K * H)*P_pri; 87 | } 88 | cout << "X_trace = " << endl << X_trace << endl; 89 | cout << "Pk_1 = " << endl << Pk_1 << endl; 90 | } -------------------------------------------------------------------------------- /trueTarget.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jeffrey-antoine/RadarDataProcessing/05b9b86f59244b776c418b50fbff5550371c4a2b/trueTarget.mat --------------------------------------------------------------------------------