Paparazzi UAS  v5.15_devel-111-g8fb4629
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
discrete_ekf_no_north.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  */
31 #include <math.h>
32 #include <stdio.h> // needed for the printf statements
33 
34 enum ekf_statein {x12, y12, z1, z2, u1, v1, u2, v2, gam};
36 
37 void extractPhiGamma(float **inmat, float **phi, float **gamma, int m, int n_a, int n_b)
38 {
39  int totalsize = m + n_b;
40  for (int i = 0; i < m; i++) {
41  for (int j = 0; j < totalsize; j++) {
42  if (j < n_a) {
43  phi[i][j] = inmat[i][j];
44  } else {
45  gamma[i][j - n_a] = inmat[i][j];
46  }
47  }
48  }
49 }
50 
51 /*
52  * Creates a combined matrix o = [ a[m][n_a] b[m][n_b] ;
53  * 0[m][n_a] 0[m][n_b] ]
54  */
55 void float_mat_combine(float **a, float **b, float **o, int m, int n_a, int n_b)
56 {
57  int totalsize = m + n_b;
58  for (int i = 0; i < totalsize; i++) {
59  for (int j = 0; j < totalsize; j++) {
60  if (i < m && j < n_a) {
61  o[i][j] = a[i][j];
62  } else if (i < m) {
63  o[i][j] = b[i][j - n_a];
64  } else {
65  o[i][j] = 0.0;
66  }
67  }
68  }
69 }
70 
71 /*
72  * Continuous to discrete transition matrix
73  */
74 void c2d(int m, int nA, int nB, float **Fx, float **G, float dt, float **phi, float **gamma)
75 {
76 
77  int totalsize = m + nB;
78  float combmat[totalsize][totalsize];
79  float expm[totalsize][totalsize];
80 
81  MAKE_MATRIX_PTR(_Fx, Fx, EKF_N);
82  MAKE_MATRIX_PTR(_G, G, EKF_M);
83  MAKE_MATRIX_PTR(_phi, phi, m);
84  MAKE_MATRIX_PTR(_gamma, gamma, m);
85  MAKE_MATRIX_PTR(_combmat, combmat, totalsize);
86  MAKE_MATRIX_PTR(_expm, expm, totalsize);
87 
88  float_mat_scale(_Fx, dt, m, nA);
89  float_mat_scale(_G, dt, m, nB);
90 
91  float_mat_combine(_Fx, _G, _combmat, m, nA, nB);
92  float_mat_exp(_combmat, _expm, totalsize);
93  extractPhiGamma(_expm, _phi, _gamma, m, nA, nB);
94 }
95 
96 /*
97  * Continuous time state transition equation
98  * state is: {x_rel,y_rel,h1,h2,u1,v1,u2,v2,gamma}
99  */
100 
101 void discrete_ekf_no_north_fsym(float *statein, float *input, float *output)
102 {
103  output[0] = input[r1m] * statein[y12] - statein[u1] + statein[u2] * cosf(statein[gam]) - statein[v2] * sinf(
104  statein[gam]);
105  output[1] = -input[r1m] * statein[x12] - statein[v1] + statein[u2] * sinf(statein[gam]) + statein[v2] * cosf(
106  statein[gam]);
107  output[2] = 0;
108  output[3] = 0;
109  output[4] = input[u1dm] + input[r1m] * statein[v1];
110  output[5] = input[v1dm] - input[r1m] * statein[u1];
111  output[6] = input[u2dm] + input[r2m] * statein[v2];
112  output[7] = input[v2dm] - input[r2m] * statein[u2];
113  output[8] = input[r2m] - input[r1m];
114 }
115 
116 /*
117  * Measurement equation, measures range, height1, height2, u1, v1, u2, v2
118  */
119 void discrete_ekf_no_north_hsym(float *statein, float *output)
120 {
121  output[0] = powf(powf((statein[z1] - statein[z2]), 2.0) + powf(statein[x12], 2.0) + powf(statein[y12], 2.0), 0.5);
122  output[1] = statein[z1];
123  output[2] = statein[z2];
124  output[3] = statein[u1];
125  output[4] = statein[v1];
126  output[5] = statein[u2];
127  output[6] = statein[v2];
128 }
129 
130 void discrete_ekf_no_north_Fx(float *statein, float *input, float **output)
131 {
132  MAKE_MATRIX_PTR(_output, output, EKF_N);
133  float_mat_zero(_output, EKF_N, EKF_N);
134  output[0][1] = input[r1m];
135  output[0][4] = -1;
136  output[0][6] = cosf(statein[gam]);
137  output[0][7] = -sinf(statein[gam]);
138  output[0][8] = -statein[v2] * cosf(statein[gam]) - statein[u2] * sinf(statein[gam]);
139  output[1][0] = -input[r1m];
140  output[1][5] = -1;
141  output[1][6] = sinf(statein[gam]);
142  output[1][7] = cosf(statein[gam]);
143  output[1][8] = statein[u2] * cosf(statein[gam]) - statein[v2] * sinf(statein[gam]);
144  output[4][5] = input[r1m];
145  output[5][4] = -input[r1m];
146  output[6][7] = input[r2m];
147  output[7][6] = -input[r2m];
148 }
149 
150 void discrete_ekf_no_north_G(float *statein, float **output)
151 {
152  MAKE_MATRIX_PTR(_output, output, EKF_N);
153  float_mat_zero(_output, EKF_N, EKF_L);
154  output[0][4] = statein[y12];
155  output[1][4] = -statein[x12];
156  output[4][0] = 1;
157  output[4][4] = statein[v1];
158  output[5][1] = 1;
159  output[5][4] = -statein[u1];
160  output[6][2] = 1;
161  output[6][4] = statein[v2];
162  output[7][3] = 1;
163  output[7][4] = -statein[u2];
164  output[8][4] = -1;
165  output[8][5] = 1;
166 }
167 
168 void discrete_ekf_no_north_Hx(float *statein, float **output)
169 {
170  MAKE_MATRIX_PTR(_output, output, EKF_N);
171  float_mat_zero(_output, EKF_M, EKF_N);
172  output[0][0] = statein[x12] / (powf(powf(statein[z1] - statein[z2], 2.0) + powf(statein[x12], 2.0) + powf(statein[y12],
173  2.0), 0.5));
174  output[0][1] = statein[y12] / (powf(powf(statein[z1] - statein[z2], 2.0) + powf(statein[x12], 2.0) + powf(statein[y12],
175  2.0), 0.5));
176  output[0][2] = (2 * statein[z1] - 2 * statein[z2]) / (2 * powf(powf(statein[z1] - statein[z2], 2.0) + powf(statein[x12],
177  2.0) + powf(statein[y12], 2.0), 0.5));
178  output[0][3] = -(2 * statein[z1] - 2 * statein[z2]) / (2 * powf(powf(statein[z1] - statein[z2],
179  2.0) + powf(statein[x12],
180  2.0) + powf(statein[y12], 2.0), 0.5));
181  output[1][2] = 1;
182  output[2][3] = 1;
183  output[3][4] = 1;
184  output[4][5] = 1;
185  output[5][6] = 1;
186  output[6][7] = 1;
187 }
188 
189 /*
190  * Initialize the filter
191  */
193 {
194  MAKE_MATRIX_PTR(_P, filter->P, EKF_N);
195  MAKE_MATRIX_PTR(_Q, filter->Q, EKF_N);
196  MAKE_MATRIX_PTR(_R, filter->R, EKF_M);
197 
198  float_mat_diagonal_scal(_P, 16, EKF_N); // P Matrix
199  float_mat_diagonal_scal(_Q, powf(2, 2), EKF_N); // Q Matrix [inputs: a1x, a1y, a2x, a2y, r1, r2]
200  filter->Q[4][4] = powf(0.2, 2);
201  filter->Q[5][5] = powf(0.2, 2);
202 
203  float_mat_diagonal_scal(_R, 0.7, EKF_M); // R Matrix [range, h1, h2, u1, v1, u2, v2]
204  filter->R[0][0] = powf(0.5, 2);
205  filter->R[1][1] = powf(0.2, 2);
206  filter->R[2][2] = powf(0.2, 2);
207 
208  float_vect_zero(filter->X, EKF_N); // Initial state
209  filter->X[0] = 1.0; // Initial X estimate
210  filter->X[1] = 1.0; // Initial Y estimate
211  filter->X[2] = -1.0;
212  filter->X[3] = -1.0;
213  filter->dt = 0.1; // Initial Est. time difference
214 }
215 
216 /*
217  * Perform the prediction step
218  */
220 {
221  float dX[EKF_N];
222 
223  MAKE_MATRIX_PTR(_tmp1, filter->tmp1, EKF_N);
224  MAKE_MATRIX_PTR(_tmp2, filter->tmp2, EKF_N);
225  MAKE_MATRIX_PTR(_tmp3, filter->tmp3, EKF_N);
226  MAKE_MATRIX_PTR(_tmp4, filter->tmp4, EKF_N);
227  MAKE_MATRIX_PTR(_P, filter->P, EKF_N);
228  MAKE_MATRIX_PTR(_Q, filter->Q, EKF_N);
229  MAKE_MATRIX_PTR(_H, filter->H, EKF_N);
230  MAKE_MATRIX_PTR(_G, filter->G, EKF_N);
231  MAKE_MATRIX_PTR(_Gamma, filter->Gamma, EKF_N);
232  MAKE_MATRIX_PTR(_Phi, filter->Phi, EKF_N);
233  MAKE_MATRIX_PTR(_Fx, filter->Fx, EKF_N);
234 
235  discrete_ekf_no_north_fsym(filter->X, U, dX);
236  discrete_ekf_no_north_Fx(filter->X, U, _Fx); // State transition matrix
237  discrete_ekf_no_north_G(filter->X, _G); // Input transition matrix
238 
239  float_vect_scale(dX, filter->dt, EKF_N);
240  float_vect_sum(filter->Xp, filter->X, dX, EKF_N);
241 
242  c2d(EKF_N, EKF_N, EKF_L, _Fx, _G, filter->dt, _Phi, _Gamma);
243 
244  float_mat_mul(_tmp1, _Phi, _P, EKF_N, EKF_N, EKF_N); // tmp1 <- Phi*P
245  float_mat_transpose_square(_Phi, EKF_N); // tmp2 <- Phi'
246  float_mat_mul(_tmp3, _tmp1, _Phi, EKF_N, EKF_N, EKF_N); // tmp3 <- Phi*P*Phi'
247 
248  float_mat_mul(_tmp1, _Gamma, _Q, EKF_N, EKF_L, EKF_L); // tmp1 <- Gamma*Q
249  float_mat_transpose(_tmp2, _Gamma, EKF_N, EKF_L); // tmp2 <- Gamma'
250  float_mat_mul(_tmp4, _tmp1, _tmp2, EKF_N, EKF_L, EKF_N); // tmp4 <- Gamma*Q*Gamma
251 
252  float_mat_sum(_P, _tmp3, _tmp4, EKF_N, EKF_N); // P <- Phi*P*Phi' + Gamma*Q*Gamma'
253 
254  discrete_ekf_no_north_hsym(filter->Xp, filter->Zp);
255  discrete_ekf_no_north_Hx(filter->Xp, _H);
256 }
257 
258 /* Perform the update step
259 
260  Get Kalman Gain
261  P12 = P * H';
262  K = P12/(H * P12 + R);
263 
264  Update x
265  x = x_p + K * (z - z_p);
266 
267  Update P
268  P = (eye(numel(x)) - K * H) * P;
269 */
271 {
272  MAKE_MATRIX_PTR(_tmp1, filter->tmp1, EKF_N);
273  MAKE_MATRIX_PTR(_tmp2, filter->tmp2, EKF_N);
274  MAKE_MATRIX_PTR(_tmp3, filter->tmp3, EKF_N);
275  MAKE_MATRIX_PTR(_P, filter->P, EKF_N);
276  MAKE_MATRIX_PTR(_H, filter->H, EKF_M);
277  MAKE_MATRIX_PTR(_Ht, filter->Ht, EKF_N);
278  MAKE_MATRIX_PTR(_R, filter->R, EKF_M);
279 
280  // E = H * P * H' + R
281  float_mat_transpose(_Ht, _H, EKF_M, EKF_N); // Ht = H'
282  float_mat_mul(_tmp2, _P, _Ht, EKF_N, EKF_N, EKF_M); // tmp2 = P*Ht = P*H'
283  float_mat_mul(_tmp1, _H, _tmp2, EKF_M, EKF_N, EKF_M); // tmp1 = H*P*H'
284  float_mat_sum(_tmp3, _tmp1, _R, EKF_M, EKF_M); // E = tmp1(=H*P*H') + R
285 
286  // K = P * H' * inv(E)
287  float_mat_invert(_tmp1, _tmp3, EKF_M); // tmp1 = inv(E)
288  float_mat_mul(_tmp3, _tmp2, _tmp1, EKF_N, EKF_M, EKF_M); // K(tmp3) = tmp2*tmp1
289 
290  // P = P - K * H * P
291  float_mat_mul(_tmp1, _tmp3, _H, EKF_N, EKF_M, EKF_N); // tmp1 = K*H
292  float_mat_mul(_tmp2, _tmp1, _P, EKF_N, EKF_N, EKF_N); // tmp3 = K*H*P
293  float_mat_diff(_P, _P, _tmp2, EKF_N, EKF_N); // P - K*H*P
294 
295  // X = X + K * err
296  float err[EKF_M];
297  float dx_err[EKF_N];
298 
299  float_vect_diff(err, Z, filter->Zp, EKF_M); // err = Z - Zp
300  float_mat_vect_mul(dx_err, _tmp3, err, EKF_N, EKF_M); // dx_err = K*err
301  float_vect_sum(filter->X, filter->Xp, dx_err, EKF_N); // X = Xp + dx_err
302 }
#define EKF_N
Definition: discrete_ekf.h:33
void float_mat_exp(float **a, float **o, int n)
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
void discrete_ekf_no_north_G(float *statein, float **output)
void extractPhiGamma(float **inmat, float **phi, float **gamma, int m, int n_a, int n_b)
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void discrete_ekf_no_north_predict(struct discrete_ekf_no_north *filter, float *U)
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)
float tmp3[EKF_N][EKF_N]
static void float_mat_diff(float **o, float **a, float **b, int m, int n)
o = a - b
void discrete_ekf_no_north_fsym(float *statein, float *input, float *output)
float Fx[EKF_N][EKF_N]
void discrete_ekf_no_north_Fx(float *statein, float *input, float **output)
float tmp1[EKF_N][EKF_N]
Paparazzi floating point algebra.
void discrete_ekf_no_north_hsym(float *statein, float *output)
void discrete_ekf_no_north_update(struct discrete_ekf_no_north *filter, float *Z)
static void float_vect_zero(float *a, const int n)
a = 0
static void float_mat_zero(float **a, int m, int n)
a = 0
void discrete_ekf_no_north_new(struct discrete_ekf_no_north *filter)
static void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
o = a * b
float Phi[EKF_N][EKF_N]
float tmp4[EKF_N][EKF_N]
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
void float_mat_combine(float **a, float **b, float **o, int m, int n_a, int n_b)
#define EKF_L
#define EKF_M
Definition: discrete_ekf.h:34
float Ht[EKF_N][EKF_M]
void c2d(int m, int nA, int nB, float **Fx, float **G, float dt, float **phi, float **gamma)
static void float_vect_scale(float *a, const float s, const int n)
a *= s
float tmp2[EKF_N][EKF_N]
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
void discrete_ekf_no_north_Hx(float *statein, float **output)
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
float Gamma[EKF_N][EKF_L]
static void float_mat_scale(float **a, float k, int m, int n)
a *= k, where k is a scalar value