Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
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
186static 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];
202 float HinvS_tmp[6][3];
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];
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.
uint16_t foo
Definition main_demo5.c:58
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 float K[9]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.