-
Notifications
You must be signed in to change notification settings - Fork 3
/
SRCKF.m
159 lines (94 loc) · 4.28 KB
/
SRCKF.m
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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
classdef SRCKF
properties
Q;
R;
Qsqrt;
Rsqrt;
nx;
xkk1;
Skk1;
xkk;
Skk;
end
methods
% Constructor
function obj = SRCKF(Q, R, nx)
obj.Q = Q;
obj.R = R;
obj.Qsqrt = sqrt(Q);
obj.Rsqrt = sqrt(R);
obj.nx = nx;
end
% Propagation
function obj = Predict(obj, xkk, Skk)
obj.xkk1 = xkk;
[~, Skk1] = qr([Skk obj.Qsqrt]',0);
obj.Skk1 = Skk1';
end
% Correction mathematically more elegant
function obj = Update2(obj, z)
Rsqrt = obj.Rsqrt;
xkk1 = obj.xkk1;
Skk1 = obj.Skk1;
%%%========================================================================
%%% Althought algebraically equivalent to Update.m, Update2 is
%%% mathematically more elegant (Read `Hybrid CKF')
%%%========================================================================
%%%========================================================================
%%% Genrate a set of Cubature Points
%%%========================================================================
nx = 2; %state vector dimension
nz = 2; %mst vector dimension
nPts = 2*nx;
CPtArray = sqrt(nPts/2)*[eye(nx) -eye(nx)];
%%%========================================================================
Xi = repmat(xkk1,1,nPts) + Skk1*CPtArray;
Zi = MstEq(Xi);
zkk1 = sum(Zi,2)/nPts; % predicted Measurement
X = (Xi-repmat(xkk1,1,nPts))/sqrt(nPts);
Z = (Zi-repmat(zkk1,1,nPts))/sqrt(nPts);
[foo,S] = qr([Z Rsqrt; X zeros(nx,nz)]',0);
S = S';
A = S(1:nz,1:nz); % Square-root Innovations Covariance
B = S(nz+1:end,1:nz);
C = S(nz+1:end,nz+1:end);
G = B/A; % Cubature Kalman Gain
obj.xkk = xkk1 + G*(z-zkk1);
obj.Skk = C;
end
% Correction
function obj = Update(obj, z)
Rsqrt = obj.Rsqrt;
xkk1 = obj.xkk1;
Skk1 = obj.Skk1;
%%%========================================================================
%%% Genrate a set of Cubature Points
%%%========================================================================
nx = obj.nx; % state vector dimension
nPts = 2*nx; % No. of Cubature Points
CPtArray = sqrt(nPts/2)*[eye(nx) -eye(nx)];
%%%========================================================================
Xi = repmat(xkk1,1,nPts) + Skk1*CPtArray;
Zi = MstEq(Xi);
zkk1 = sum(Zi,2)/nPts; % predicted Measurement
X = (Xi-repmat(xkk1,1,nPts))/sqrt(nPts);
Z = (Zi-repmat(zkk1,1,nPts))/sqrt(nPts);
[~, Szz] = qr([Z Rsqrt]',0);
Szz = Szz'; % Square-root Innovations Covariance
Pxz = X*Z';
G = (Pxz/Szz')/Szz; % Cubature Kalman Gain
xkk = xkk1 + G*(z - zkk1);
[~, Skk] = qr([(X - G*Z) G*Rsqrt]',0);
Skk = Skk';
% Save objects
obj.xkk = xkk;
obj.Skk = Skk;
end
end
methods(Static)
function z = MstEq(x)
z = [ 0.8*cos(x(1,:)) - 0.2*cos(x(1,:) + x(2,:)) ;
0.8*sin(x(1,:)) - 0.2*sin(x(1,:) + x(2,:)) ];
end
end
end