RadarDataProcessing/main.cpp at master · jeffrey-antoine/RadarDataProcessing · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
#include <iostream>
#include <Eigen/Dense>
#include <ctime>
#include <random>
using namespace Eigen;
using namespace std;

double generate_random(double sigma)
{
	static default_random_engine e(time(0));

	static normal_distribution<double> n(0, sigma);

	return n(e);
}
int main()
{

	Matrix<double, 2, 151> trueTarget;
	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;
	// generate measurement
	double sigma = 10; // set noise variance


	Array<double, 2, 151> noise;
	for (int i = 0; i < 151*2; i++)
	{
		noise(i) = generate_random(sigma);
	}


	//cout << noise << endl;
	Matrix<double, 2, 151> measure = trueTarget + noise.matrix();
	cout << measure << endl;

	double T = 1;

	Matrix4d A;
	A << 1, 0, T, 0,
		0, 1, 0, T,
		0, 0, 1, 0,
		0, 0, 0, 1;
	Matrix<double,4,2> B;
	B << 0.5*T*T, 0,
		0, 0.5*T*T,
		T,0,
		0, T;
	cout << "B=" << endl << B << endl;
	Matrix<double, 2, 4>H;
	H << 1, 0, 0, 0, 0, 1, 0, 0;
	cout << "H=" << endl << H << endl;
	Vector4d xk_1;
	Matrix4d Pk_1;
	xk_1 << measure(0,0), measure(0,1), 0, 0;
	Pk_1 << sigma * sigma, 0, 0, 0,
			0, sigma*sigma, 0, 0,
			0, 0, sigma, 0,
			0, 0, 0, sigma;
	Matrix2d R;
	R<<sigma*sigma,0,
		0,sigma*sigma;
	Matrix<double, 4, 2> K;
	Matrix4d EMatrix;
	EMatrix.setIdentity();

	cout<<"EMatrix = " << endl << EMatrix << endl;
	Matrix2d Qpro;
	Qpro.setIdentity();
	Matrix4d Q = B * Qpro * B.transpose();
	Matrix<double, 4, 151> X_trace;

	for (int i = 1; i < 150; i++)
	{
		Vector4d x_pri = A * xk_1;
		Matrix4d P_pri = A * Pk_1*A.transpose()+Q;

		Matrix<double,4,2> temp = P_pri * H.transpose();
		MatrixXd temp2 = H * P_pri*H.transpose() + R;
		K = temp * temp2.inverse();
		Vector2d measure_temp;
		measure_temp << measure(0, i), measure(1, i);

		xk_1 = x_pri + K * (measure_temp - H * x_pri);
		X_trace.col(i) = xk_1;

		Pk_1 = (EMatrix - K * H)*P_pri;
	}
	cout << "X_trace = " << endl << X_trace << endl;
	cout << "Pk_1 = " << endl << Pk_1 << endl;
}