Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
34enum ekf_statein {x12, y12, z1, z2, u1, v1, u2, v2, gam};
36
37void 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 */
55void 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 */
74void c2d(int m, int nA, int nB, float **Fx, float **G, float dt, float **phi, float **gamma)
75{
76
77 int totalsize = m + nB;
79 float expm[totalsize][totalsize];
80
83 MAKE_MATRIX_PTR(_phi, phi, m);
87
88 float_mat_scale(_Fx, dt, m, nA);
89 float_mat_scale(_G, dt, m, nB);
90
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
101void 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 */
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
130void discrete_ekf_no_north_Fx(float *statein, float *input, float **output)
131{
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
151{
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
169{
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);
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);
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
#define EKF_M
void float_mat_combine(float **a, float **b, float **o, int m, int n_a, int n_b)
void discrete_ekf_no_north_Hx(float *statein, float **output)
void discrete_ekf_no_north_hsym(float *statein, float *output)
void discrete_ekf_no_north_fsym(float *statein, float *input, float *output)
void discrete_ekf_no_north_predict(struct discrete_ekf_no_north *filter, float *U)
void discrete_ekf_no_north_new(struct discrete_ekf_no_north *filter)
void discrete_ekf_no_north_Fx(float *statein, float *input, float **output)
void extractPhiGamma(float **inmat, float **phi, float **gamma, int m, int n_a, int n_b)
void discrete_ekf_no_north_update(struct discrete_ekf_no_north *filter, float *Z)
void discrete_ekf_no_north_G(float *statein, float **output)
void c2d(int m, int nA, int nB, float **Fx, float **G, float dt, float **phi, float **gamma)
#define EKF_L
float Gamma[EKF_N][EKF_L]
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_vect_scale(float *a, const float s, const int n)
a *= s
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
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
void float_mat_exp(float **a, float **o, int n)
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
static void float_mat_scale(float **a, float k, int m, int n)
a *= k, where k is a scalar value
uint16_t foo
Definition main_demo5.c:58
Paparazzi floating point algebra.
float b
Definition wedgebug.c:202