Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 */
#define EKF_N
Definition: discrete_ekf.h:33
float tmp2[EKF_N][EKF_N]
Definition: discrete_ekf.h:47
float R[EKF_M][EKF_M]
Definition: discrete_ekf.h:42
float H[EKF_M][EKF_N]
Definition: discrete_ekf.h:43
float Q[EKF_N][EKF_N]
Definition: discrete_ekf.h:41
void discrete_ekf_predict(struct discrete_ekf *filter)
Definition: discrete_ekf.c:74
void discrete_ekf_new(struct discrete_ekf *filter)
Definition: discrete_ekf.c:32
void linear_measure(float *X, float *Y, float **H)
Definition: discrete_ekf.c:164
void discrete_ekf_update(struct discrete_ekf *filter, float *y)
Definition: discrete_ekf.c:113
float Zp[EKF_M]
Definition: discrete_ekf.h:39
float tmp3[EKF_N][EKF_N]
Definition: discrete_ekf.h:48
float X[EKF_N]
Definition: discrete_ekf.h:37
#define EKF_M
Definition: discrete_ekf.h:34
float tmp1[EKF_N][EKF_N]
Definition: discrete_ekf.h:46
static struct FloatVect3 H
#define A
float P[EKF_N][EKF_N]
Definition: discrete_ekf.h:40
float Xp[EKF_N]
Definition: discrete_ekf.h:38
float Ht[EKF_N][EKF_M]
Definition: discrete_ekf.h:44
void linear_filter(float *X, float dt, float *dX, float **A)
Definition: discrete_ekf.c:148