Paparazzi UAS  v6.2_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 */
discrete_ekf_no_north::tmp2
float tmp2[EKF_N][EKF_N]
Definition: discrete_ekf_no_north.h:55
discrete_ekf_no_north::tmp3
float tmp3[EKF_N][EKF_N]
Definition: discrete_ekf_no_north.h:56
discrete_ekf_no_north::Gamma
float Gamma[EKF_N][EKF_L]
Definition: discrete_ekf_no_north.h:51
discrete_ekf_no_north_hsym
void discrete_ekf_no_north_hsym(float *statein, float *output)
Definition: discrete_ekf_no_north.c:119
discrete_ekf_no_north_G
void discrete_ekf_no_north_G(float *statein, float **output)
Definition: discrete_ekf_no_north.c:150
b
float b
Definition: wedgebug.c:202
discrete_ekf_no_north::Xp
float Xp[EKF_N]
Definition: discrete_ekf_no_north.h:42
EKF_N
#define EKF_N
Definition: discrete_ekf_no_north.h:36
extractPhiGamma
void extractPhiGamma(float **inmat, float **phi, float **gamma, int m, int n_a, int n_b)
Definition: discrete_ekf_no_north.c:37
discrete_ekf_no_north::H
float H[EKF_M][EKF_N]
Definition: discrete_ekf_no_north.h:47
discrete_ekf_no_north::Ht
float Ht[EKF_N][EKF_M]
Definition: discrete_ekf_no_north.h:49
discrete_ekf_no_north::Q
float Q[EKF_N][EKF_N]
Definition: discrete_ekf_no_north.h:45
EKF_L
#define EKF_L
Definition: discrete_ekf_no_north.h:38
c2d
void c2d(int m, int nA, int nB, float **Fx, float **G, float dt, float **phi, float **gamma)
Definition: discrete_ekf_no_north.c:74
discrete_ekf_no_north::P
float P[EKF_N][EKF_N]
Definition: discrete_ekf_no_north.h:44
discrete_ekf_no_north_Hx
void discrete_ekf_no_north_Hx(float *statein, float **output)
Definition: discrete_ekf_no_north.c:168
discrete_ekf_no_north::tmp1
float tmp1[EKF_N][EKF_N]
Definition: discrete_ekf_no_north.h:54
discrete_ekf_no_north::dt
float dt
Definition: discrete_ekf_no_north.h:59
EKF_M
#define EKF_M
Definition: discrete_ekf_no_north.h:37
discrete_ekf_no_north::tmp4
float tmp4[EKF_N][EKF_N]
Definition: discrete_ekf_no_north.h:57
discrete_ekf_no_north_new
void discrete_ekf_no_north_new(struct discrete_ekf_no_north *filter)
Definition: discrete_ekf_no_north.c:192
discrete_ekf_no_north
Definition: discrete_ekf_no_north.h:40
discrete_ekf_no_north_Fx
void discrete_ekf_no_north_Fx(float *statein, float *input, float **output)
Definition: discrete_ekf_no_north.c:130
discrete_ekf_no_north_fsym
void discrete_ekf_no_north_fsym(float *statein, float *input, float *output)
Definition: discrete_ekf_no_north.c:101
discrete_ekf_no_north::X
float X[EKF_N]
Definition: discrete_ekf_no_north.h:41
discrete_ekf_no_north::Zp
float Zp[EKF_M]
Definition: discrete_ekf_no_north.h:43
discrete_ekf_no_north_predict
void discrete_ekf_no_north_predict(struct discrete_ekf_no_north *filter, float *U)
Definition: discrete_ekf_no_north.c:219
discrete_ekf_no_north::Phi
float Phi[EKF_N][EKF_N]
Definition: discrete_ekf_no_north.h:50
float_mat_combine
void float_mat_combine(float **a, float **b, float **o, int m, int n_a, int n_b)
Definition: discrete_ekf_no_north.c:55
discrete_ekf_no_north::G
float G[EKF_N][EKF_L]
Definition: discrete_ekf_no_north.h:48
discrete_ekf_no_north::R
float R[EKF_M][EKF_M]
Definition: discrete_ekf_no_north.h:46
discrete_ekf_no_north::Fx
float Fx[EKF_N][EKF_N]
Definition: discrete_ekf_no_north.h:52
discrete_ekf_no_north_update
void discrete_ekf_no_north_update(struct discrete_ekf_no_north *filter, float *y)
Definition: discrete_ekf_no_north.c:270