Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
linear_kalman_filter.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 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  */
28 
29 
39 {
40  if (n > KF_MAX_STATE_SIZE || c > KF_MAX_CMD_SIZE || m > KF_MAX_MEAS_SIZE) {
41  filter->n = 0;
42  filter->c = 0;
43  filter->m = 0;
44  return false; // invalide sizes;
45  }
46  filter->n = n;
47  filter->c = c;
48  filter->m = m;
49 
50  // Matrix
51  MAKE_MATRIX_PTR(_A, filter->A, n);
52  float_mat_zero(_A, n, n);
53  MAKE_MATRIX_PTR(_B, filter->B, n);
54  float_mat_zero(_B, n, c);
55  MAKE_MATRIX_PTR(_C, filter->C, m);
56  float_mat_zero(_C, m, n);
57  MAKE_MATRIX_PTR(_P, filter->P, n);
58  float_mat_zero(_P, n, n);
59  MAKE_MATRIX_PTR(_Q, filter->Q, n);
60  float_mat_zero(_Q, n, n);
61  MAKE_MATRIX_PTR(_R, filter->R, m);
62  float_mat_zero(_R, m, m);
63 
64  // Vector
65  float_vect_zero(filter->X, n);
66 
67  return true;
68 }
69 
78 void linear_kalman_filter_predict(struct linear_kalman_filter *filter, float *U)
79 {
80  float AX[filter->n];
81  float BU[filter->n];
82  float tmp[filter->n][filter->n];
83 
84  MAKE_MATRIX_PTR(_A, filter->A, filter->n);
85  MAKE_MATRIX_PTR(_B, filter->B, filter->n);
86  MAKE_MATRIX_PTR(_P, filter->P, filter->n);
87  MAKE_MATRIX_PTR(_Q, filter->Q, filter->n);
88  MAKE_MATRIX_PTR(_tmp, tmp, filter->n);
89 
90  // X = A * X + B * U
91  float_mat_vect_mul(AX, _A, filter->X, filter->n, filter->n);
92  float_mat_vect_mul(BU, _B, U, filter->n, filter->c);
93  float_vect_sum(filter->X, AX, BU, filter->n);
94 
95  // P = A * P * A' + Q
96  float_mat_mul(_tmp, _A, _P, filter->n, filter->n, filter->n); // A * P
97  float_mat_mul_transpose(_P, _tmp, _A, filter->n, filter->n, filter->n); // * A'
98  float_mat_sum(_P, _P, _Q, filter->n, filter->n); // + Q
99 }
100 
111 extern void linear_kalman_filter_update(struct linear_kalman_filter *filter, float *Y)
112 {
113  float S[filter->m][filter->m];
114  float K[filter->n][filter->m];
115  float tmp1[filter->n][filter->m];
116  float tmp2[filter->n][filter->n];
117 
118  MAKE_MATRIX_PTR(_P, filter->P, filter->n);
119  MAKE_MATRIX_PTR(_C, filter->C, filter->m);
120  MAKE_MATRIX_PTR(_R, filter->R, filter->m);
121  MAKE_MATRIX_PTR(_S, S, filter->m);
122  MAKE_MATRIX_PTR(_K, K, filter->n);
123  MAKE_MATRIX_PTR(_tmp1, tmp1, filter->n);
124  MAKE_MATRIX_PTR(_tmp2, tmp2, filter->n);
125 
126  // S = Cd * P * Cd' + R
127  float_mat_mul_transpose(_tmp1, _P, _C, filter->n, filter->n, filter->m); // P * C'
128  float_mat_mul(_S, _C, _tmp1, filter->m, filter->n, filter->m); // C *
129  float_mat_sum(_S, _S, _R, filter->m, filter->m); // + R
130 
131  // K = P * Cd' * inv(S)
132  float_mat_invert(_S, _S, filter->m); // inv(S) in place
133  float_mat_mul(_K, _tmp1, _S, filter->n, filter->m, filter->m); // tmp1 {P*C'} * inv(S)
134 
135  // P = P - K * C * P
136  float_mat_mul(_tmp2, _K, _C, filter->n, filter->m, filter->n); // K * C
137  float_mat_mul_copy(_tmp2, _tmp2, _P, filter->n, filter->n, filter->n); // * P
138  float_mat_diff(_P, _P, _tmp2, filter->n, filter->n); // P - K*H*P
139 
140  // X = X + K * err
141  float err[filter->n];
142  float dx_err[filter->n];
143 
144  float_mat_vect_mul(err, _C, filter->X, filter->m, filter->n); // C * X
145  float_vect_diff(err, Y, err, filter->m); // err = Y - C * X
146  float_mat_vect_mul(dx_err, _K, err, filter->n, filter->m); // K * err
147  float_vect_sum(filter->X, filter->X, dx_err, filter->n); // X + dx_err
148 }
149 
float_mat_mul_transpose
static void float_mat_mul_transpose(float **o, float **a, float **b, int m, int n, int l)
o = a * b'
Definition: pprz_algebra_float.h:737
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
linear_kalman_filter::c
uint8_t c
command vector size (<= KF_MAX_CMD_SIZE)
Definition: linear_kalman_filter.h:58
linear_kalman_filter.h
linear_kalman_filter::A
float A[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
dynamic matrix
Definition: linear_kalman_filter.h:48
linear_kalman_filter::R
float R[KF_MAX_MEAS_SIZE][KF_MAX_MEAS_SIZE]
measurement covariance noise
Definition: linear_kalman_filter.h:53
float_mat_invert
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.
Definition: pprz_algebra_float.c:883
MAKE_MATRIX_PTR
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
Definition: pprz_algebra_float.h:638
KF_MAX_CMD_SIZE
#define KF_MAX_CMD_SIZE
Definition: linear_kalman_filter.h:38
linear_kalman_filter::B
float B[KF_MAX_STATE_SIZE][KF_MAX_CMD_SIZE]
command matrix
Definition: linear_kalman_filter.h:49
linear_kalman_filter::n
uint8_t n
state vector size (<= KF_MAX_STATE_SIZE)
Definition: linear_kalman_filter.h:57
linear_kalman_filter
Definition: linear_kalman_filter.h:46
float_mat_diff
static void float_mat_diff(float **o, float **a, float **b, int m, int n)
o = a - b
Definition: pprz_algebra_float.h:679
KF_MAX_MEAS_SIZE
#define KF_MAX_MEAS_SIZE
Definition: linear_kalman_filter.h:43
pprz_algebra_float.h
Paparazzi floating point algebra.
linear_kalman_filter_predict
void linear_kalman_filter_predict(struct linear_kalman_filter *filter, float *U)
Prediction step.
Definition: linear_kalman_filter.c:78
KF_MAX_STATE_SIZE
#define KF_MAX_STATE_SIZE
Definition: linear_kalman_filter.h:33
linear_kalman_filter::C
float C[KF_MAX_MEAS_SIZE][KF_MAX_STATE_SIZE]
observation matrix
Definition: linear_kalman_filter.h:50
float_vect_zero
static void float_vect_zero(float *a, const int n)
a = 0
Definition: pprz_algebra_float.h:542
linear_kalman_filter::X
float X[KF_MAX_STATE_SIZE]
estimated state X
Definition: linear_kalman_filter.h:55
float_mat_zero
static void float_mat_zero(float **a, int m, int n)
a = 0
Definition: pprz_algebra_float.h:650
float_mat_vect_mul
static void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
o = a * b
Definition: pprz_algebra_float.h:785
float_mat_mul_copy
static void float_mat_mul_copy(float **o, float **a, float **b, int m, int n, int l)
o = a * b
Definition: pprz_algebra_float.h:761
float_vect_diff
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
Definition: pprz_algebra_float.h:563
linear_kalman_filter_update
void linear_kalman_filter_update(struct linear_kalman_filter *filter, float *Y)
Update step.
Definition: linear_kalman_filter.c:111
linear_kalman_filter_init
bool linear_kalman_filter_init(struct linear_kalman_filter *filter, uint8_t n, uint8_t c, uint8_t m)
Init all matrix and vectors to zero.
Definition: linear_kalman_filter.c:38
K
static float K[9]
Definition: undistort_image.c:41
float_mat_sum
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
Definition: pprz_algebra_float.h:670
linear_kalman_filter::P
float P[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
state covariance matrix
Definition: linear_kalman_filter.h:51
linear_kalman_filter::Q
float Q[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
proces covariance noise
Definition: linear_kalman_filter.h:52
float_mat_mul
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
Definition: pprz_algebra_float.h:718
mesonh.mesonh_atmosphere.Y
int Y
Definition: mesonh_atmosphere.py:44
linear_kalman_filter::m
uint8_t m
measurement vector size (<= KF_MAX_MEAS_SIZE)
Definition: linear_kalman_filter.h:59
float_vect_sum
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
Definition: pprz_algebra_float.h:556