Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
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
53extern void linear_filter(float *X, float dt, float *dX, float **A);
54extern void linear_measure(float *X, float *Y, float **H);
55
56extern void discrete_ekf_new(struct discrete_ekf *filter);
57extern void discrete_ekf_predict(struct discrete_ekf *filter);
58extern void discrete_ekf_update(struct discrete_ekf *filter, float *y);
59
60#endif /* DISCRETE_EKF_H */
float X[EKF_N]
float R[EKF_M][EKF_M]
void discrete_ekf_update(struct discrete_ekf *filter, float *y)
float tmp1[EKF_N][EKF_N]
void discrete_ekf_predict(struct discrete_ekf *filter)
void linear_filter(float *X, float dt, float *dX, float **A)
void discrete_ekf_new(struct discrete_ekf *filter)
float tmp3[EKF_N][EKF_N]
float Q[EKF_N][EKF_N]
float Ht[EKF_N][EKF_M]
#define EKF_N
float H[EKF_M][EKF_N]
float P[EKF_N][EKF_N]
void linear_measure(float *X, float *Y, float **H)
float Xp[EKF_N]
float tmp2[EKF_N][EKF_N]
#define EKF_M
float Zp[EKF_M]
#define A
static struct FloatVect3 H
uint16_t foo
Definition main_demo5.c:58