Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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
32 void 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);
45  float_mat_diagonal_scal(_Q, powf(0.3, 2.f), 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);
50  float_mat_diagonal_scal(_R, powf(0.1, 2.f), 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 */
74 void discrete_ekf_predict(struct discrete_ekf *filter)
75 {
76  float dX[EKF_N];
77 
78  MAKE_MATRIX_PTR(_tmp1, filter->tmp1, EKF_N);
79  MAKE_MATRIX_PTR(_tmp2, filter->tmp2, EKF_N);
80  MAKE_MATRIX_PTR(_tmp3, filter->tmp3, EKF_N);
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
96  float_mat_transpose_square(_tmp1, EKF_N); // tmp1 = A'
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 */
113 void 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 */
148 void linear_filter(float *X, float dt, float *dX, float **A)
149 {
150  // dX
151  float_vect_zero(dX, EKF_N);
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 */
164 void 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 };
#define EKF_N
Definition: discrete_ekf.h:33
static void float_vect_sum(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...
static void float_mat_transpose_square(float **a, int n)
transpose square matrix
float tmp2[EKF_N][EKF_N]
Definition: discrete_ekf.h:47
float R[EKF_M][EKF_M]
Definition: discrete_ekf.h:42
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
float H[EKF_M][EKF_N]
Definition: discrete_ekf.h:43
float Q[EKF_N][EKF_N]
Definition: discrete_ekf.h:41
void discrete_ekf_predict(struct discrete_ekf *filter)
Definition: discrete_ekf.c:74
void linear_measure(float *X, float *Z, float **H)
Definition: discrete_ekf.c:164
static void float_mat_diff(float **o, float **a, float **b, int m, int n)
o = a - b
void linear_filter(float *X, float dt, float *dX, float **A)
Definition: discrete_ekf.c:148
void discrete_ekf_update(struct discrete_ekf *filter, float *Z)
Definition: discrete_ekf.c:113
float Zp[EKF_M]
Definition: discrete_ekf.h:39
Paparazzi floating point algebra.
static void float_vect_zero(float *a, const int n)
a = 0
float tmp3[EKF_N][EKF_N]
Definition: discrete_ekf.h:48
static void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
o = a * b
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
float X[EKF_N]
Definition: discrete_ekf.h:37
#define EKF_M
Definition: discrete_ekf.h:34
float tmp1[EKF_N][EKF_N]
Definition: discrete_ekf.h:46
unsigned char uint8_t
Definition: types.h:14
static struct FloatVect3 H
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
#define A
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
void discrete_ekf_new(struct discrete_ekf *filter)
Definition: discrete_ekf.c:32
float P[EKF_N][EKF_N]
Definition: discrete_ekf.h:40
float Xp[EKF_N]
Definition: discrete_ekf.h:38
float Ht[EKF_N][EKF_M]
Definition: discrete_ekf.h:44
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b