-
Notifications
You must be signed in to change notification settings - Fork 0
/
kalman-notes.txt
96 lines (80 loc) · 3.33 KB
/
kalman-notes.txt
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
91
92
93
94
95
96
// TODO Perform kDefaultServerPortman filtering on the target.
float enemy_pos_x = 0;
float enemy_pos_y = 50;
float delta_t = 0.5;
float c = 0.2;
//-----------------------------------------------------------------------------
// Constants initialized beforehand
mat F; // 6x6
mat sigma_x; // 6x6
mat H; // 2x6
mat sigma_z; // 2x2
mat F_transpose; // 6x6
mat H_tranpose; // 6x2
mat I; // 6x6
// velocity given delta t
float vdt = delta_t;
// acceleration given delta t
float adt = (delta_t * delta_t) / 2;
// System transition matrix
F << 1 << vdt << adt << 0 << 0 << 0 << endr
<< 0 << 1 << vdt << 0 << 0 << 0 << endr
<< 0 << -c << 1 << 0 << 0 << 0 << endr
<< 0 << 0 << 0 << 1 << vdt << adt << endr
<< 0 << 0 << 0 << 0 << 1 << vdt << endr
<< 0 << 0 << 0 << 0 << -c << 1 << endr;
// Noise
sigma_x << 0.1 << 0 << 0 << 0 << 0 << 0 << endr
<< 0 << 0.1 << 0 << 0 << 0 << 0 << endr
<< 0 << 0 << 100 << 0 << 0 << 0 << endr
<< 0 << 0 << 0 << 0.1 << 0 << 0 << endr
<< 0 << 0 << 0 << 0 << 0.1 << 0 << endr
<< 0 << 0 << 0 << 0 << 0 << 100 << endr;
// Observation matrix that selects out the two position values from state vector
H << 1 << 0 << 0 << 0 << 0 << 0 << endr
<< 0 << 0 << 0 << 1 << 0 << 0 << endr;
// Covariance matrix
sigma_z << 25 << 0 << endr
<< 0 << 25 << endr;
// F transpose
F_transpose = F.t();
// H tranpose
H_tranpose = H.t();
// Identity matrix
I.eye(6,6);
// TODO not sure where this needs to be initialized
colvec X_t(6); // 6x1
// State vector
X_t << enemy_pos_x << endr
<< 0.1 << endr
<< 0.1 << endr
<< enemy_pos_y << endr
<< 0.1 << endr
<< 0.1 << endr;
//-----------------------------------------------------------------------------
// Initialized at start of each run
colvec mu(6); // 6x1
mat sigma; // 6x6
// Mean estimate of the state vector
mu.fill(0);
// Covariance for the state vector
sigma << 100 << 0 << 0 << 0 << 0 << 0 << endr
<< 0 << 0.1 << 0 << 0 << 0 << 0 << endr
<< 0 << 0 << 0.1 << 0 << 0 << 0 << endr
<< 0 << 0 << 0 << 100 << 0 << 0 << endr
<< 0 << 0 << 0 << 0 << 0.1 << 0 << endr
<< 0 << 0 << 0 << 0 << 0 << 0.1 << endr;
//-----------------------------------------------------------------------------
// The following change with each time step
mat A = (F * sigma * F_transpose) + sigma_x; // 6x6
colvec take_out_vec_Xt(6); // 6x1
colvec take_out_vec_Zt(2); // 2x1
take_out_vec_Xt.fill(1);
take_out_vec_Zt.fill(1);
// TODO the following extracts values from sigma_x/z, instead of adding
// sigma_x/z like the original equation shows?
X_t = (F * X_t) + (sigma_x * take_out_vec_Xt); // 6x1
colvec Z_t = (H * X_t) + (sigma_z * take_out_vec_Zt); // 2x1
mat K_t_plus_1 = (A * H_tranpose * ((H * A * H_tranpose) + sigma_z)).i(); // 6x2
mu = (F * mu) + (K_t_plus_1 * (Z_t - (H * F * mu))); // 6x1 mut+1 = ...*mut*...
sigma = (I - (K_t_plus_1 * H)) * A; // 6x6 sigmat+1 = ...*sigmat*..