Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
simple_kinematic_kalman.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi 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, or (at your option)
9  * any later version.
10  *
11  * paparazzi 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 paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
29 #include <math.h>
30 
31 void simple_kinematic_kalman_init(struct SimpleKinematicKalman *kalman, float P0_pos, float P0_speed, float Q_sigma2,
32  float r, float dt)
33 {
34  int i, j;
35  const float dt2 = dt * dt;
36  const float dt3 = dt2 * dt / 2.f;
37  const float dt4 = dt2 * dt2 / 4.f;
38  for (i = 0; i < SIMPLE_KINEMATIC_KALMAN_DIM; i++) {
39  kalman->state[i] = 0.f; // don't forget to call set_state before running the filter for better results
40  for (j = 0; j < SIMPLE_KINEMATIC_KALMAN_DIM; j++) {
41  kalman->P[i][j] = 0.f;
42  kalman->Q[i][j] = 0.f;
43  }
44  }
45  for (i = 0; i < SIMPLE_KINEMATIC_KALMAN_DIM; i += 2) {
46  kalman->P[i][i] = P0_pos;
47  kalman->P[i + 1][i + 1] = P0_speed;
48  kalman->Q[i][i] = Q_sigma2 * dt4;
49  kalman->Q[i + 1][i] = Q_sigma2 * dt3;
50  kalman->Q[i][i + 1] = Q_sigma2 * dt3;
51  kalman->Q[i + 1][i + 1] = Q_sigma2 * dt2;
52  }
53  kalman->r = r;
54  kalman->dt = dt;
55 
56  // kalman->F = {{1, dt, 0, 0 , 0, 0},
57  // {0, 1 , 0, 0, 0, 0},
58  // {0, 0, 1, dt 0, 0},
59  // {0, 0, 0, 1 0, 0},
60  // {0, 0, 0, 0, 1 ,dt}
61  // {0, 0, 0, 0, 0, 1 }};
62 
63  for (int i = 0; i < SIMPLE_KINEMATIC_KALMAN_DIM; i++) {
64  for (int j = 0; j < SIMPLE_KINEMATIC_KALMAN_DIM; j++) {
65  if (i == j) {
66  kalman->F[i][i] = 1;
67  } else if (i % 2 == 0 && j == i + 1) {
68  kalman->F[i][j] = dt;
69  } else {
70  kalman->F[i][j] = 0;
71  }
72  }
73  }
74 
75  // kalman->Hp = {{1,0,0,0,0,0},
76  // {0,0,1,0,0,0},
77  // {0,0,0,0,1,0}};
78 
79  for (int i = 0; i < SIMPLE_KINEMATIC_KALMAN_DIM / 2; i++) {
80  for (int j = 0; j < SIMPLE_KINEMATIC_KALMAN_DIM; j++) {
81  if (2 * i == j) {
82  kalman->Hp[i][j] = 1.f;
83  } else {
84  kalman->Hp[i][j] = 0.f;
85  }
86  // horizontal and vertical speed can be updated separately, init matrix to zero
87  kalman->Hs[i][j] = 0.f;
88  }
89  }
90 }
91 
93  struct FloatVect3 speed)
94 {
95  kalman->state[0] = pos.x;
96  kalman->state[1] = speed.x;
97  kalman->state[2] = pos.y;
98  kalman->state[3] = speed.y;
99  kalman->state[4] = pos.z;
100  kalman->state[5] = speed.z;
101 }
102 
104  struct FloatVect3 *speed)
105 {
106  pos->x = kalman->state[0];
107  pos->y = kalman->state[2];
108  pos->z = kalman->state[4];
109  speed->x = kalman->state[1];
110  speed->y = kalman->state[3];
111  speed->z = kalman->state[5];
112 }
113 
115 {
116  struct FloatVect3 pos;
117  pos.x = kalman->state[0];
118  pos.y = kalman->state[2];
119  pos.z = kalman->state[4];
120  return pos;
121 }
122 
124 {
125  struct FloatVect3 speed;
126  speed.x = kalman->state[1];
127  speed.y = kalman->state[3];
128  speed.z = kalman->state[5];
129  return speed;
130 }
131 
133 {
134  int i;
135  const float dt = kalman->dt;
136  const float dt2 = dt * dt;
137  const float dt3 = dt2 * dt / 2.f;
138  const float dt4 = dt2 * dt2 / 4.f;
139  for (i = 0; i < SIMPLE_KINEMATIC_KALMAN_DIM; i += 2) {
140  kalman->Q[i][i] = Q_sigma2 * dt4;
141  kalman->Q[i + 1][i] = Q_sigma2 * dt3;
142  kalman->Q[i][i + 1] = Q_sigma2 * dt3;
143  kalman->Q[i + 1][i + 1] = Q_sigma2 * dt2;
144  }
145  kalman->r = r;
146 }
147 
158 {
159  int i;
160  for (i = 0; i < SIMPLE_KINEMATIC_KALMAN_DIM; i += 2) {
161  // kinematic equation of the dynamic model X = F*X
162  kalman->state[i] += kalman->state[i + 1] * kalman->dt;
163 
164  // propagate covariance P = F*P*Ft + Q
165  // since F is diagonal by block, P can be updated by block here as well
166  // let's unroll the matrix operations as it is simple
167 
168  const float d_dt = kalman->P[i + 1][i + 1] * kalman->dt;
169  kalman->P[i][i] += kalman->P[i + 1][i] * kalman->dt + kalman->dt * (kalman->P[i][i + 1] + d_dt) + kalman->Q[i][i];
170  kalman->P[i][i + 1] += d_dt + kalman->Q[i][i + 1];
171  kalman->P[i + 1][i] += d_dt + kalman->Q[i + 1][i];
172  kalman->P[i + 1][i + 1] += kalman->Q[i + 1][i + 1];
173  }
174 }
175 
186 static void simple_kinematic_kalman_update(struct SimpleKinematicKalman *kalman, float **_H, float *Z)
187 {
188  float S[3][3] = {
189  {kalman->P[0][0] + kalman->r, kalman->P[0][2], kalman->P[0][4]},
190  {kalman->P[2][0], kalman->P[2][2] + kalman->r, kalman->P[2][4]},
191  {kalman->P[4][0], kalman->P[4][2], kalman->P[4][4] + kalman->r}
192  };
193 
194  if (fabsf(S[0][0]) + fabsf(S[1][1]) + fabsf(S[2][2]) < 1e-5) {
195  return; // don't inverse S if it is too small
196  }
197 
198  // prepare variables and pointers
199  MAKE_MATRIX_PTR(_S, S, 3);
200  float invS[3][3];
201  MAKE_MATRIX_PTR(_invS, invS, 3);
202  float HinvS_tmp[6][3];
203  MAKE_MATRIX_PTR(_HinvS_tmp, HinvS_tmp, 6);
204  float Ht[6][3];
205  MAKE_MATRIX_PTR(_Ht, Ht, 6);
206  float K[6][3];
207  MAKE_MATRIX_PTR(_K, K, 6);
208  float HX_tmp[3];
209  float Z_HX[3];
210  float K_ZHX_tmp[6];
211  float KH_tmp[6][6];
212  MAKE_MATRIX_PTR(_KH_tmp, KH_tmp, 6);
213  MAKE_MATRIX_PTR(_P, kalman->P, 6);
214  float P_tmp[6][6];
215 
216  // finally compute gain and correct state
217  float_mat_invert(_invS, _S, 3); // S^-1
218  float_mat_transpose(_Ht, _H, 3, 6); // Ht
219  float_mat_mul(_HinvS_tmp, _Ht, _invS, 6, 3, 3); // Ht * S^-1
220  float_mat_mul(_K, _P, _HinvS_tmp, 6, 6, 3); // P * Ht * S^-1
221  float_mat_vect_mul(HX_tmp, _H, kalman->state, 3, 6); // H * X
222  float_vect_diff(Z_HX, Z, HX_tmp, 3); // Z - H * X
223  float_mat_vect_mul(K_ZHX_tmp, _K, Z_HX, 6, 3); // K * (Z - H * X)
224  float_vect_add(kalman->state, K_ZHX_tmp, 6); // X + K * (Z - H * X)
225 
226  // precompute K*H and store current P
227  float_mat_mul(_KH_tmp, _K, _H, 6, 3, 6);
228  for (int i = 0; i < 6; i++) {
229  for (int j = 0; j < 6; j++) {
230  P_tmp[i][j] = kalman->P[i][j];
231  }
232  }
233  // correct covariance P = (I-K*H)*P = P - K*H*P
234  for (int i = 0; i < 6; i++) {
235  for (int j = 0; j < 6; j++) {
236  for (int k = 0; k < 6; k++) {
237  kalman->P[i][j] -= KH_tmp[i][k] * P_tmp[k][j];
238  }
239  }
240  }
241 }
242 
250 {
251  // measurement vector
252  float Z[3] = { pos.x, pos.y, pos.z };
253  // position observation matrix
254  MAKE_MATRIX_PTR(_H, kalman->Hp, 3);
255 
256  // call update
258 }
259 
267 {
268  // measurement vector
269  float Z[3] = { speed.x, speed.y, speed.z };
271  // update horizontal speed
272  kalman->Hs[0][1] = 1.f;
273  kalman->Hs[1][3] = 1.f;
274  kalman->Hs[2][5] = 0.f;
275  } else if (type == SIMPLE_KINEMATIC_KALMAN_SPEED_VERTICAL) {
276  // update vertical speed
277  kalman->Hs[0][1] = 0.f;
278  kalman->Hs[1][3] = 0.f;
279  kalman->Hs[2][5] = 1.f;
280  } else {
281  // update 3D speed
282  kalman->Hs[0][1] = 1.f;
283  kalman->Hs[1][3] = 1.f;
284  kalman->Hs[2][5] = 1.f;
285  }
286  // speed observation matrix
287  MAKE_MATRIX_PTR(_H, kalman->Hs, 3);
288 
289  // call update
291 }
292 
static void float_vect_add(float *a, const float *b, const int n)
a += b
static void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
o = a * b
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_mat_invert(float **o, float **mat, int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
static void simple_kinematic_kalman_update(struct SimpleKinematicKalman *kalman, float **_H, float *Z)
generic correction step
struct FloatVect3 simple_kinematic_kalman_get_pos(struct SimpleKinematicKalman *kalman)
Get current pos.
void simple_kinematic_kalman_get_state(struct SimpleKinematicKalman *kalman, struct FloatVect3 *pos, struct FloatVect3 *speed)
Get current state.
void simple_kinematic_kalman_update_noise(struct SimpleKinematicKalman *kalman, float Q_sigma2, float r)
Update process and measurement noises.
void simple_kinematic_kalman_predict(struct SimpleKinematicKalman *kalman)
propagate dynamic model
void simple_kinematic_kalman_set_state(struct SimpleKinematicKalman *kalman, struct FloatVect3 pos, struct FloatVect3 speed)
Set initial state vector.
void simple_kinematic_kalman_update_pos(struct SimpleKinematicKalman *kalman, struct FloatVect3 pos)
correction step for position
void simple_kinematic_kalman_init(struct SimpleKinematicKalman *kalman, float P0_pos, float P0_speed, float Q_sigma2, float r, float dt)
Init SimpleKinematicKalman internal struct.
void simple_kinematic_kalman_update_speed(struct SimpleKinematicKalman *kalman, struct FloatVect3 speed, uint8_t type)
correction step for speed
struct FloatVect3 simple_kinematic_kalman_get_speed(struct SimpleKinematicKalman *kalman)
Get current speed.
#define SIMPLE_KINEMATIC_KALMAN_DIM
#define SIMPLE_KINEMATIC_KALMAN_SPEED_HORIZONTAL
float Hp[SIMPLE_KINEMATIC_KALMAN_DIM/2][SIMPLE_KINEMATIC_KALMAN_DIM]
observation matrix for position
float r
measurement noise (assumed the same for all anchors)
float state[SIMPLE_KINEMATIC_KALMAN_DIM]
state vector
float Q[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM]
process noise matrix
float P[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM]
covariance matrix
#define SIMPLE_KINEMATIC_KALMAN_SPEED_VERTICAL
float F[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM]
dynamic matrix
float dt
prediction step (in seconds)
float Hs[SIMPLE_KINEMATIC_KALMAN_DIM/2][SIMPLE_KINEMATIC_KALMAN_DIM]
observation matrix for speed
static struct SimpleKinematicKalman kalman
Definition: tag_tracking.c:170
static float K[9]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98