libnxter  0.1
KalmanFilter.nxc
Go to the documentation of this file.
1 /* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
2 /*
3  KalmanFilter.nxc
4  Copyright (C) 2008 Naba Kumar <naba@gnome.org>
5 
6  This program is free software; you can redistribute it and/or modify
7  it under the terms of the GNU General Public License as published by
8  the Free Software Foundation; either version 2 of the License, or
9  (at your option) any later version.
10 
11  This program is distributed in the hope that it will be useful,
12  but WITHOUT ANY WARRANTY; without even the implied warranty of
13  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  GNU General Public License for more details.
15 
16  You should have received a copy of the GNU General Public License
17  along with this program; if not, write to the Free Software
18  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19 */
20 
21 #ifndef _KALMAN_FILTER_H_
22 #define _KALMAN_FILTER_H_
23 
24 //#define ENABLE_DEBUG
25 //#define ENABLE_TEST
26 
27 #include "Debug.nxc"
28 #include "Matrix.nxc"
29 #include "RandomNorm.nxc"
30 
31 #define KALMAN_GAIN_PRECISION 1000 /* gain precision */
32 
44 {
53  long Ks;
54  bool unityModel;
55 };
56 
66  Matrix &Q, Matrix &R, Matrix &P0, Matrix &X0)
67 {
68  filter.unityModel = false;
69  filter.A = A;
70  filter.B = B;
71  filter.H = H;
72  filter.Q = Q;
73  filter.R = R;
74  filter.P = P0;
75  filter.X = X0;
76 }
77 
90  Matrix &P0, Matrix &X0)
91 {
92  filter.unityModel = true;
93  filter.Q = Q;
94  filter.R = R;
95  filter.P = P0;
96  filter.X = X0;
97 }
98 
102 inline void KalmanFilterGetX(KalmanFilter &filter, Matrix &X)
103 {
104  X = filter.X;
105 }
106 
112 inline void KalmanFilterGetP(KalmanFilter &filter, Matrix &P)
113 {
114  P = filter.P;
115 }
116 
124 {
125  Matrix BU, Xp, Yr, tmpV;
126  Matrix Pp, Sr, tmpM;
127 
128  if (filter.unityModel) /* A = B = H = I */
129  {
130  /* Predict step Xp = X + U */
131  Xp = filter.X;
132  MatrixAdd(Xp, U);
133 
134  /* Predict step: Pp = P + Q */
135  Pp = filter.P;
136  MatrixAdd(Pp, filter.Q);
137 
138  /* Measurement residual: Yr = Z - Xp */
139  Yr = Z;
140  MatrixSubtract(Yr, Xp);
141 
142  /* Residual covariance: Sr = Pp + R */
143  Sr = Pp;
144  MatrixAdd(Sr, filter.R);
145 
146  /* (Optimal) K = Pp * Inv(Sr) */
147  filter.Ks = MatrixInverse(Sr, KALMAN_GAIN_PRECISION,
148  tmpM); /* Sr = Inv(Sr) */
149  MatrixMultiply(Pp, tmpM, filter.K); /* K = Pp * Inv(Sr) */
150  }
151  else /* Complete model */
152  {
153  /* Predict step: Xp = A * X + B * U */
154 
155  MatrixMultiply(filter.B, U, BU); /* BU = B * U */
156  MatrixMultiply(filter.A, filter.X, Xp); /* Xp = A * X */
157  MatrixAdd(Xp, BU); /* Xp = A * X + B * U */
158 
159  /* Predict step: Pp = A * P * T(A) + Q */
160 
161  MatrixMultiply(filter.A, filter.P, Pp); /* Pp = A * P */
162  MatrixTranspose(filter.A); /* A = T(A) */
163  MatrixMultiply(Pp, filter.A, tmpM); /* Pp = A * P * T(A) */
164  Pp = tmpM;
165  MatrixAdd(Pp, filter.Q); /* Pp = A * P * T(A) + Q */
166  MatrixTranspose(filter.A); /* Restore original A */
167 
168  /* Measurement residual: Yr = Z - H * Xp */
169 
170  MatrixMultiply(filter.H, Xp, tmpV); /* tmp = H * Xp */
171  Yr = Z;
172  MatrixSubtract(Yr, tmpV); /* Yr = Z - H * Xp */
173 
174  /* Residual covariance: Sr = H * Pp * T(H) + R */
175 
176  MatrixMultiply(filter.H, Pp, Sr); /* Sr = H * Pp */
177  MatrixTranspose(filter.H); /* H = T(H) */
178  MatrixMultiply(Sr, filter.H, tmpM); /* Sr = H * Pp * T(H) */
179  Sr = tmpM;
180  MatrixAdd(Sr, filter.R); /* Sr = H * Pp * T(H) + R */
181 
182  /* (Optimal) K = Pp * T(H) * Ks * Inv(Sr) */
183 
184  MatrixMultiply(Pp, filter.H, filter.K); /* K = Pp * H */
185  filter.Ks = MatrixInverse(Sr, KALMAN_GAIN_PRECISION,
186  tmpM); /* tmpM = Inv(Sr) */
187  MatrixMultiply(filter.K, tmpM, Sr); /* K = Pp * H * Inv(Sr) */
188  filter.K = Sr;
189  MatrixTranspose(filter.H); /* Restore original H */
190  }
191 
192  /* At this point K is automatically scalled by Ks because of
193  * Inv(Sr) being scaled. This will prevent K from falling
194  * to 0. If it is 0, it will be set to I.
195  */
196  if (MatrixIsNull(filter.K))
197  MatrixInitIdentity(filter.K, filter.K.rows, filter.K.cols);
198 
199  /* Posteriori update: X = Xp + K * Yr / Ks*/
200 
201  filter.X = Xp; /* X = Xp */
202  MatrixMultiply(filter.K, Yr, Xp); /* tmp = K * Yr */
203  MatrixReduce(Xp, filter.Ks); /* tmp = K * Yr / Ks */
204  MatrixAdd(filter.X, Xp); /* X = Xp + K * Yr / Ks */
205  /* Xp no more useful after this */
206 
207  /* Posteriori update: P = (Ks * I - K * H) * Pp / Ks */
208 
209  MatrixInitDiagonal(tmpM, filter.K.rows,
210  filter.K.cols,
211  filter.Ks); /* tmp = Ks * I */
212  if (filter.unityModel)
213  {
214  MatrixSubtract(tmpM, filter.K); /* tmp = (Ks * I - K) */
215  }
216  else
217  {
218  MatrixMultiply(filter.K, filter.H,
219  filter.P); /* P = (K * H) */
220  MatrixSubtract(tmpM, filter.P); /* tmp = (Ks * I - K * H) */
221  }
222  MatrixMultiply(tmpM, Pp, filter.P); /* P = (Ks * I - K * H) * Pp / Ks*/
223  MatrixReduce(filter.P, filter.Ks); /* P = (Ks * I - K) * Pp / Ks */
224 }
225 
226 /* #define ENABLE_TEST */
227 #ifdef ENABLE_TEST
228 
229 #define DIM 1
230 #define SetPoint 1500
231 #define InitialPoint 0
232 #define InitialP 0
233 #define QPoint 200
234 #define RPoint 500
235 
236 int initialX[] = {InitialPoint, InitialPoint, InitialPoint, InitialPoint};
237 int initialU[] = {0, 0, 0, 0};
238 int lastPoint = 0;
239 int lastXPos = 0;
240 
241 void PlotKalman(Matrix &z, Matrix &x, Matrix &p, int yMin, int yMax, int xPos)
242 {
243  int za = ((MatrixGet(z, 0, 0) - yMin) * 63) / (yMax - yMin);
244  int xa = ((MatrixGet(x, 0, 0) - yMin) * 63) / (yMax - yMin);
245  int pa = MatrixGet(p, 0, 0);
246 
247  if (xa > 63) xa = 63;
248  if (xa < 0) xa = 0;
249  if (xPos < lastXPos) lastXPos = 0;
250 
251  PointOut(xPos, za, false);
252  CircleOut(xPos, za, 2, false);
253  LineOut(lastXPos, lastPoint, xPos, xa, false);
254 
255  int deviation = Sqrt(pa);
256  LineOut(xPos, (xa - deviation), xPos, (xa + deviation), false);
257  lastPoint = xa;
258  lastXPos = xPos;
259 }
260 
266 void TestKalmanFilter()
267 {
268  Matrix x, u, z;
269  int xl;
270  KalmanFilter filter;
271  Matrix A, B, H, Q, R, P;
272 
273  MatrixInitIdentity(A, DIM, DIM);
274  MatrixInitIdentity(B, DIM, DIM);
275  MatrixInitIdentity(H, DIM, DIM);
276  MatrixInitDiagonal(Q, DIM, DIM, QPoint);
277  MatrixInitDiagonal(R, DIM, DIM, RPoint);
278  MatrixInitDiagonal(P, DIM, DIM, InitialP);
279 
280  MatrixInitElements(x, DIM, 1, initialX);
281  MatrixInitElements(u, DIM, 1, initialU);
282  MatrixInit(z, DIM, 1);
283 
284  //KalmanFilterInit(filter, A, B, H, Q, R, P, x);
285  KalmanFilterInitUnity(filter, Q, R, P, x);
286  xl = InitialPoint;
287 
288  while(true)
289  {
290  ClearScreen();
291  int jl = 0;
292  for (int j = 5; j < 110; j += 5)
293  {
294  for (int k = 0; k < DIM; k++)
295  {
296  if (k == 0)
297  MatrixSet(z, k, 0, SetPoint + RandomNorm(100));
298  else
299  MatrixSet(z, k, 0, 0);
300  break;
301  }
302  int t1 = CurrentTick();
303  KalmanFilterStep(filter, u, z);
304  int t2 = CurrentTick();
305  KalmanFilterGetX(filter, x);
306  KalmanFilterGetP(filter, P);
307  PlotKalman(z, x, P, SetPoint - 700, SetPoint + 700, j);
308 
309  TextOut(0, LCD_LINE1, "---------------------", false);
310  NumOut (0, LCD_LINE1, MatrixGet(x, 0, 0), false);
311  NumOut (55, LCD_LINE1, t2 - t1, false);
312  }
313  }
314 }
315 
316 /*
317 task main()
318 {
319  TestKalmanFilter();
320 }
321 */
322 #endif
323 #endif
324 
long MatrixInverse(Matrix matrix, long precision, Matrix &retInverse)
Finds the inverse of a matrix. The inverted matix is scalled by the returned scale factor...
Definition: Matrix.nxc:554
void MatrixInitDiagonal(Matrix &matrix, int rows, int cols, long value)
Initializes a matrix to a diagonal matrix of size rows x cols, with all elements except diagonal elem...
Definition: Matrix.nxc:119
The matrix class. Don't access the members directly. Use the macros MIJ() or MI() or the methods prov...
Definition: Matrix.nxc:64
bool MatrixIsNull(Matrix &matrix)
Returns true if the matrix is NULL, i.e. all elemants are 0.
Definition: Matrix.nxc:166
void MatrixSet(Matrix &matrix, int row, int col, long value)
Sets the given value to the element in matrix given by row x col. This is exactly same as assigning t...
Definition: Matrix.nxc:142
void MatrixInit(Matrix &matrix, int rows, int cols)
Initializes a matrix to a matrix of size rows x cols and fills it with 0s.
Definition: Matrix.nxc:76
void MatrixMultiply(Matrix &matrixA, Matrix &matrixB, Matrix &matrixC)
Multiplies matrix A with B and stores the result in matrix C. i.e. C = A * B. Matrix C must not be sa...
Definition: Matrix.nxc:276
void MatrixAdd(Matrix &matrixA, Matrix &matrixB)
Adds matrix B to matrix A. i.e. A += B.
Definition: Matrix.nxc:250
void KalmanFilterInitUnity(KalmanFilter &filter, Matrix &Q, Matrix &R, Matrix &P0, Matrix &X0)
Initializes the kalman filter in unity mode with given paramters. Unity mode runs the filter with the...
Debugging utility macros.
int rows
Definition: Matrix.nxc:67
void KalmanFilterGetX(KalmanFilter &filter, Matrix &X)
Gets the current estimated state in the filter and returns it in X.
void KalmanFilterInit(KalmanFilter &filter, Matrix &A, Matrix &B, Matrix &H, Matrix &Q, Matrix &R, Matrix &P0, Matrix &X0)
Initializes the kalman filter with given paramters. matrix A is the state transition model...
void KalmanFilterGetP(KalmanFilter &filter, Matrix &P)
Gets the current error covariance of the current estimated state in the filter and returns it in P...
Normally distributed (Gaussian distribution) random number function.
Kalman Filter class representing the kalman model and state updates. In practice, upto only 3 dimenti...
void KalmanFilterStep(KalmanFilter &filter, Matrix &U, Matrix &Z)
Runs the filter step. matrix U is the current state control input and matrix Z is the current observa...
void MatrixInitElements(Matrix &matrix, int rows, int cols, long &elements[])
Initializes a matrix to a matrix of size rows x cols and fills it with elements from given array...
Definition: Matrix.nxc:91
Integer matrix implementation. Provides matrix algebra, cofactor, adjugate and inverse computation...
void MatrixSubtract(Matrix &matrixA, Matrix &matrixB)
Subtracts matrix B from matrix A. i.e. A -= B.
Definition: Matrix.nxc:262
void MatrixTranspose(Matrix &matrix)
Converts the given matrix into its Transpose, i.e. the matrix is flipped along its diagonal (top-left...
Definition: Matrix.nxc:179
void MatrixReduce(Matrix &matrix, long scale)
Reduces (i.e. scales down) all elements in matrix A by given reduction factor. i.e. A /= scale.
Definition: Matrix.nxc:301
int cols
Definition: Matrix.nxc:68
int RandomNorm(int scale)
Returns a random number sample from Guassian distribution scaled by the given factor (using lookup ta...
Definition: RandomNorm.nxc:64
long MatrixGet(Matrix &matrix, int row, int col)
Returns the value of element in matrix given by row x col. This is exactly same as macro MIJ()...
Definition: Matrix.nxc:133
void MatrixInitIdentity(Matrix &matrix, int rows, int cols)
Initializes a matrix to an Identity matrix of size rows x cols, with all elements except diagonal ele...
Definition: Matrix.nxc:104