Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
discrete_ekf_no_north.h
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  */
29 #ifndef DISCRETE_EKF_NO_NORTH_H
30 #define DISCRETE_EKF_NO_NORTH_H
31 
32 #include "stdlib.h"
33 #include "string.h"
34 #include "math.h"
35 
36 #define EKF_N 9
37 #define EKF_M 7
38 #define EKF_L 6
39 
41  float X[EKF_N]; // state X
42  float Xp[EKF_N]; // state prediction
43  float Zp[EKF_M]; // measurement prediction
44  float P[EKF_N][EKF_N]; // state covariance matrix
45  float Q[EKF_N][EKF_N]; // proces covariance noise
46  float R[EKF_M][EKF_M]; // measurement covariance noise
47  float H[EKF_M][EKF_N]; // jacobian of the measure wrt X
48  float G[EKF_N][EKF_L]; // Noise input
49  float Ht[EKF_N][EKF_M]; // transpose of H
50  float Phi[EKF_N][EKF_N]; // Jacobian
51  float Gamma[EKF_N][EKF_L]; // Noise input
52  float Fx[EKF_N][EKF_N]; // Jacobian of state
53 
54  float tmp1[EKF_N][EKF_N];
55  float tmp2[EKF_N][EKF_N];
56  float tmp3[EKF_N][EKF_N];
57  float tmp4[EKF_N][EKF_N];
58 
59  float dt;
60 };
61 
62 
63 void extractPhiGamma(float **inmat, float **phi, float **gamma, int m, int n_a, int n_b);
64 void float_mat_combine(float **a, float **b, float **o, int m, int n_a, int n_b);
65 void c2d(int m, int nA, int nB, float **Fx, float **G, float dt, float **phi, float **gamma);
66 void discrete_ekf_no_north_fsym(float *statein, float *input, float *output);
67 void discrete_ekf_no_north_hsym(float *statein, float *output);
68 void discrete_ekf_no_north_Fx(float *statein, float *input, float **output);
69 void discrete_ekf_no_north_G(float *statein, float **output);
70 void discrete_ekf_no_north_Hx(float *statein, float **output);
71 
72 extern void discrete_ekf_no_north_new(struct discrete_ekf_no_north *filter);
73 extern void discrete_ekf_no_north_predict(struct discrete_ekf_no_north *filter, float *U);
74 extern void discrete_ekf_no_north_update(struct discrete_ekf_no_north *filter, float *y);
75 
76 #endif /* DISCRETE_EKF_NO_NORTH_H */
void float_mat_combine(float **a, float **b, float **o, int m, int n_a, int n_b)
#define EKF_L
void discrete_ekf_no_north_Hx(float *statein, float **output)
void discrete_ekf_no_north_hsym(float *statein, float *output)
float tmp1[EKF_N][EKF_N]
float tmp3[EKF_N][EKF_N]
float tmp4[EKF_N][EKF_N]
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)
float Fx[EKF_N][EKF_N]
#define EKF_N
void discrete_ekf_no_north_Fx(float *statein, float *input, float **output)
float Phi[EKF_N][EKF_N]
float tmp2[EKF_N][EKF_N]
void extractPhiGamma(float **inmat, float **phi, float **gamma, int m, int n_a, int n_b)
float Ht[EKF_N][EKF_M]
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)
float Gamma[EKF_N][EKF_L]
#define EKF_M
void discrete_ekf_no_north_update(struct discrete_ekf_no_north *filter, float *y)
float b
Definition: wedgebug.c:202