Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
discrete_ekf.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  */
26 #ifndef DISCRETE_EKF_H
27 #define DISCRETE_EKF_H
28 
29 #include "stdlib.h"
30 #include "string.h"
31 #include "math.h"
32 
33 #define EKF_N 7
34 #define EKF_M 6
35 
36 struct discrete_ekf {
37  float X[EKF_N]; // state X
38  float Xp[EKF_N]; // state prediction
39  float Zp[EKF_M]; // measurement prediction
40  float P[EKF_N][EKF_N]; // state covariance matrix
41  float Q[EKF_N][EKF_N]; // proces covariance noise
42  float R[EKF_M][EKF_M]; // measurement covariance noise
43  float H[EKF_M][EKF_N]; // jacobian of the measure wrt X
44  float Ht[EKF_N][EKF_M]; // transpose of H
45 
46  float tmp1[EKF_N][EKF_N];
47  float tmp2[EKF_N][EKF_N];
48  float tmp3[EKF_N][EKF_N];
49 
50  float dt;
51 };
52 
53 extern void linear_filter(float *X, float dt, float *dX, float **A);
54 extern void linear_measure(float *X, float *Y, float **H);
55 
56 extern void discrete_ekf_new(struct discrete_ekf *filter);
57 extern void discrete_ekf_predict(struct discrete_ekf *filter);
58 extern void discrete_ekf_update(struct discrete_ekf *filter, float *y);
59 
60 #endif /* DISCRETE_EKF_H */
discrete_ekf::tmp2
float tmp2[EKF_N][EKF_N]
Definition: discrete_ekf.h:47
EKF_N
#define EKF_N
Definition: discrete_ekf.h:33
discrete_ekf::R
float R[EKF_M][EKF_M]
Definition: discrete_ekf.h:42
H
static struct FloatVect3 H
Definition: mag_calib_ukf.c:127
discrete_ekf_predict
void discrete_ekf_predict(struct discrete_ekf *filter)
Definition: discrete_ekf.c:74
discrete_ekf::H
float H[EKF_M][EKF_N]
Definition: discrete_ekf.h:43
discrete_ekf::Q
float Q[EKF_N][EKF_N]
Definition: discrete_ekf.h:41
discrete_ekf_new
void discrete_ekf_new(struct discrete_ekf *filter)
Definition: discrete_ekf.c:32
discrete_ekf_update
void discrete_ekf_update(struct discrete_ekf *filter, float *y)
Definition: discrete_ekf.c:113
linear_measure
void linear_measure(float *X, float *Y, float **H)
Definition: discrete_ekf.c:164
discrete_ekf::Zp
float Zp[EKF_M]
Definition: discrete_ekf.h:39
A
#define A
Definition: pprz_geodetic_utm.h:44
discrete_ekf::dt
float dt
Definition: discrete_ekf.h:50
discrete_ekf::tmp1
float tmp1[EKF_N][EKF_N]
Definition: discrete_ekf.h:46
discrete_ekf::tmp3
float tmp3[EKF_N][EKF_N]
Definition: discrete_ekf.h:48
discrete_ekf::X
float X[EKF_N]
Definition: discrete_ekf.h:37
EKF_M
#define EKF_M
Definition: discrete_ekf.h:34
discrete_ekf::P
float P[EKF_N][EKF_N]
Definition: discrete_ekf.h:40
discrete_ekf::Xp
float Xp[EKF_N]
Definition: discrete_ekf.h:38
discrete_ekf
Definition: discrete_ekf.h:36
linear_filter
void linear_filter(float *X, float dt, float *dX, float **A)
Definition: discrete_ekf.c:148
mesonh.mesonh_atmosphere.Y
int Y
Definition: mesonh_atmosphere.py:44
discrete_ekf::Ht
float Ht[EKF_N][EKF_M]
Definition: discrete_ekf.h:44
mesonh.mesonh_atmosphere.X
int X
Definition: mesonh_atmosphere.py:43