Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
discrete_ekf.c
Go to the documentation of this file.
1/*
2 * Copyright (C) Mario Coppola
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#include <math.h>
29#include <stdio.h> // needed for the printf statements
30
31// Weights are based on: Coppola et al, "On-board Communication-based Relative Localization for Collision Avoidance in Micro Air Vehicle teams", 2017
32void discrete_ekf_new(struct discrete_ekf *filter)
33{
34 // P Matrix
35 MAKE_MATRIX_PTR(_P, filter->P, EKF_N);
37 filter->P[2][2] = 0.1;
38 filter->P[3][3] = 0.1;
39 filter->P[4][4] = 0.1;
40 filter->P[5][5] = 0.1;
41 filter->P[6][6] = 0.1;
42
43 // Q Matrix
44 MAKE_MATRIX_PTR(_Q, filter->Q, EKF_N);
46 filter->Q[0][0] = 0.01;
47 filter->Q[1][1] = 0.01;
48
49 MAKE_MATRIX_PTR(_R, filter->R, EKF_M);
51 filter->R[0][0] = 0.2;
52
53 // Initial assumptions
54 float_vect_zero(filter->X, EKF_N);
55 filter->X[0] = 1.0; // filter->X[0] and/or filter->[1] cannot be = 0
56 filter->X[1] = 1.0; // filter->X[0] and/or filter->[1] cannot be = 0
57 filter->dt = 0.1;
58}
59
60/* Perform the prediction step
61
62 Predict state
63 x_p = f(x);
64 A = Jacobian of f(x)
65
66 Predict P
67 P = A * P * A' + Q;
68
69 Predict measure
70 z_p = h(x_p)
71 H = Jacobian of h(x)
72
73*/
75{
76 float dX[EKF_N];
77
81 MAKE_MATRIX_PTR(_H, filter->H, EKF_N);
82 MAKE_MATRIX_PTR(_P, filter->P, EKF_N);
83 MAKE_MATRIX_PTR(_Q, filter->Q, EKF_N);
84
85 // Fetch dX and A given X and dt and input u
86 linear_filter(filter->X, filter->dt, dX, _tmp1); // (note: _tmp1 = A)
87
88 // Get state prediction Xp = X + dX
89 float_vect_sum(filter->Xp, filter->X, dX, EKF_N);
90
91 // Get measurement prediction (Zp) and Jacobian (H)
92 linear_measure(filter->Xp, filter->Zp, _H);
93
94 // P = A * P * A' + Q
95 float_mat_mul(_tmp2, _tmp1, _P, EKF_N, EKF_N, EKF_N); // tmp2(=A*P) = A(=_tmp1)*_P
97 float_mat_mul(_tmp3, _tmp2, _tmp1, EKF_N, EKF_N, EKF_N); // tmp3 = tmp2*tmp1 = A*P * A'
98 float_mat_sum(_P, _tmp3, _Q, EKF_N, EKF_N); // P = tmp3(=A*P*A') + Q
99}
100
101/* Perform the update step
102
103 Get Kalman Gain
104 P12 = P * H';
105 K = P12/(H * P12 + R);
106
107 Update x
108 x = x_p + K * (z - z_p);
109
110 Update P
111 P = (eye(numel(x)) - K * H) * P;
112*/
113void discrete_ekf_update(struct discrete_ekf *filter, float *Z)
114{
115 MAKE_MATRIX_PTR(_tmp1, filter->tmp1, EKF_N);
116 MAKE_MATRIX_PTR(_tmp2, filter->tmp2, EKF_N);
117 MAKE_MATRIX_PTR(_tmp3, filter->tmp3, EKF_N);
118 MAKE_MATRIX_PTR(_P, filter->P, EKF_N);
119 MAKE_MATRIX_PTR(_H, filter->H, EKF_M);
120 MAKE_MATRIX_PTR(_Ht, filter->Ht, EKF_N);
121 MAKE_MATRIX_PTR(_R, filter->R, EKF_M);
122
123 // E = H * P * H' + R
124 float_mat_transpose(_Ht, _H, EKF_M, EKF_N); // Ht = H'
125 float_mat_mul(_tmp2, _P, _Ht, EKF_N, EKF_N, EKF_M); // tmp2 = P*Ht = P*H'
126 float_mat_mul(_tmp1, _H, _tmp2, EKF_M, EKF_N, EKF_M); // tmp1 = H*P*H'
127 float_mat_sum(_tmp3, _tmp1, _R, EKF_M, EKF_M); // E = tmp1(=H*P*H') + R
128
129 // K = P * H' * inv(E)
130 float_mat_invert(_tmp1, _tmp3, EKF_M); // tmp1 = inv(E)
131 float_mat_mul(_tmp3, _tmp2, _tmp1, EKF_N, EKF_M, EKF_M); // K(tmp3) = tmp2*tmp1
132
133 // P = P - K * H * P
134 float_mat_mul(_tmp1, _tmp3, _H, EKF_N, EKF_M, EKF_N); // tmp1 = K*H
135 float_mat_mul(_tmp2, _tmp1, _P, EKF_N, EKF_N, EKF_N); // tmp3 = K*H*P
136 float_mat_diff(_P, _P, _tmp2, EKF_N, EKF_N); // P - K*H*P
137
138 // X = X + K * err
139 float err[EKF_M];
140 float dx_err[EKF_N];
141
142 float_vect_diff(err, Z, filter->Zp, EKF_M); // err = Z - Zp
143 float_mat_vect_mul(dx_err, _tmp3, err, EKF_N, EKF_M); // dx_err = K*err
144 float_vect_sum(filter->X, filter->Xp, dx_err, EKF_N); // X = Xp + dx_err
145}
146
147/* Linearized (Jacobian) filter function */
148void linear_filter(float *X, float dt, float *dX, float **A)
149{
150 // dX
152 dX[0] = -(X[2] - X[4]) * dt;
153 dX[1] = -(X[3] - X[5]) * dt;
154
155 // A(x)
157 A[0][2] = -dt;
158 A[0][4] = dt;
159 A[1][3] = -dt;
160 A[1][5] = dt;
161};
162
163/* Linearized (Jacobian) measure function */
164void linear_measure(float *X, float *Z, float **H)
165{
166 uint8_t row, col;
167 Z[0] = sqrt(pow(X[0], 2.f) + pow(X[1], 2.f) + pow(X[6], 2.f));
168 Z[1] = X[2]; // x velocity of i (north)
169 Z[2] = X[3]; // y velocity of i (east)
170 Z[3] = X[4]; // x velocity of j (north)
171 Z[4] = X[5]; // y velocity of j (east)
172 Z[5] = X[6]; // Height difference
173
174 // Generate the Jacobian Matrix
175 for (row = 0 ; row < EKF_M ; row++) {
176 for (col = 0 ; col < EKF_N ; col++) {
177 // x, y, and z pos columns are affected by the range measurement
178 if ((row == 0) && (col == 0 || col == 1 || col == 6)) {
179 H[row][col] = X[col] / sqrt(pow(X[0], 2.f) + pow(X[1], 2.f) + pow(X[6], 2.f));
180 }
181
182 // All other values are 1
183 else if (((row == 1) && (col == 2)) ||
184 ((row == 2) && (col == 3)) ||
185 ((row == 3) && (col == 4)) ||
186 ((row == 4) && (col == 5)) ||
187 ((row == 5) && (col == 6))) {
188 H[row][col] = 1.f;
189 }
190
191 else {
192 H[row][col] = 0.f;
193 }
194 }
195 }
196
197};
void discrete_ekf_update(struct discrete_ekf *filter, float *Z)
void discrete_ekf_predict(struct discrete_ekf *filter)
void linear_filter(float *X, float dt, float *dX, float **A)
void discrete_ekf_new(struct discrete_ekf *filter)
void linear_measure(float *X, float *Z, float **H)
float X[EKF_N]
float R[EKF_M][EKF_M]
float tmp1[EKF_N][EKF_N]
float tmp3[EKF_N][EKF_N]
float Q[EKF_N][EKF_N]
float Ht[EKF_N][EKF_M]
#define EKF_N
float H[EKF_M][EKF_N]
float P[EKF_N][EKF_N]
float Xp[EKF_N]
float tmp2[EKF_N][EKF_N]
#define EKF_M
float Zp[EKF_M]
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(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 float_mat_transpose_square(float **a, int n)
transpose square matrix
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
#define A
static struct FloatVect3 H
uint16_t foo
Definition main_demo5.c:58
Paparazzi floating point algebra.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.