Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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{
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
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
111extern 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
static void float_mat_mul_transpose(float **o, float **a, float **b, int m, int n, int l)
o = a * b'
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
static void float_vect_zero(float *a, const int n)
a = 0
static void float_mat_diff(float **o, float **a, float **b, int m, int n)
o = a - b
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = 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_copy(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_mat_zero(float **a, int m, int n)
a = 0
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
#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.
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.
void linear_kalman_filter_predict(struct linear_kalman_filter *filter, float *U)
Prediction step.
void linear_kalman_filter_update(struct linear_kalman_filter *filter, float *Y)
Update step.
#define KF_MAX_STATE_SIZE
float C[KF_MAX_MEAS_SIZE][KF_MAX_STATE_SIZE]
observation matrix
#define KF_MAX_CMD_SIZE
uint8_t n
state vector size (<= KF_MAX_STATE_SIZE)
float R[KF_MAX_MEAS_SIZE][KF_MAX_MEAS_SIZE]
measurement covariance noise
uint8_t m
measurement vector size (<= KF_MAX_MEAS_SIZE)
#define KF_MAX_MEAS_SIZE
float X[KF_MAX_STATE_SIZE]
estimated state X
uint8_t c
command vector size (<= KF_MAX_CMD_SIZE)
float P[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
state covariance matrix
float A[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
dynamic matrix
float B[KF_MAX_STATE_SIZE][KF_MAX_CMD_SIZE]
command matrix
float Q[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
proces covariance noise
uint16_t foo
Definition main_demo5.c:58
Paparazzi floating point algebra.
static float K[9]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.