28#include "pprzlink/messages.h"
67 float ga = (float)
gps.hmsl / 1000.;
101 k->
P[0][0] = 1; k->
P[0][1] = 0; k->
P[0][2] = 0;
102 k->
P[1][0] = 0; k->
P[1][1] = 1; k->
P[1][2] = 0;
103 k->
P[2][0] = 0; k->
P[2][1] = 0; k->
P[2][2] = 0.0001;
108 k->
Ad[0][0] = 1; k->
Ad[0][1] = k->
Te; k->
Ad[0][2] = 0;
109 k->
Ad[1][0] = 0; k->
Ad[1][1] = 1; k->
Ad[1][2] = 0;
110 k->
Ad[2][0] = 0; k->
Ad[2][1] = 0; k->
Ad[2][2] = 1;
113 k->
Bd[0] = pow(k->
Te, 2) / 2;
117 k->
Md[0][0] = pow(k->
Te, 1.5) / 2; k->
Md[0][1] = 0;
118 k->
Md[1][0] = pow(k->
Te, 0.5); k->
Md[1][1] = 0;
119 k->
Md[2][0] = 0; k->
Md[2][1] = pow(k->
Te, 0.5);
140 k->
X[0] = k->
Ad[0][0] * k->
X[0] + k->
Ad[0][1] * k->
X[1] + k->
Ad[0][2] * k->
X[2] + k->
Bd[0] * accVert;
141 k->
X[1] = k->
Ad[1][0] * k->
X[0] + k->
Ad[1][1] * k->
X[1] + k->
Ad[1][2] * k->
X[2] + k->
Bd[1] * accVert;
142 k->
X[2] = k->
Ad[2][0] * k->
X[0] + k->
Ad[2][1] * k->
X[1] + k->
Ad[2][2] * k->
X[2] + k->
Bd[2] * accVert;
146 for (i = 0; i < 3; i++) {
147 for (j = 0; j < 3; j++) {
148 I[i][j] = k->
Ad[i][0] * k->
P[0][j] + k->
Ad[i][1] * k->
P[1][j] + k->
Ad[i][2] * k->
P[2][j];
152 for (i = 0; i < 3; i++) {
153 for (j = 0; j < 3; j++) {
154 k->
P[i][j] =
I[i][0] * k->
Ad[j][0] +
I[i][1] * k->
Ad[j][1] +
I[i][2] * k->
Ad[j][2];
158 for (i = 0; i < 3; i++) {
159 for (j = 0; j < 2; j++) {
160 J[i][j] = k->
Md[i][0] * k->
W[0][j] + k->
Md[i][1] * k->
W[1][j];
164 for (i = 0; i < 3; i++) {
165 for (j = 0; j < 3; j++) {
166 k->
P[i][j] = k->
P[i][j] +
J[i][0] * k->
Md[j][0] +
J[i][1] * k->
Md[j][1];
185 float Kf[3] = { 0., 0., 0. };
192 if (fabs(div) > 1e-5) {
194 Kf[0] = k->
P[0][0] / div;
195 Kf[1] = k->
P[1][0] / div;
196 Kf[2] = k->
P[2][0] / div;
200 float constante = k->
X[0];
201 for (i = 0; i < 3; i++) {
202 k->
X[i] = k->
X[i] + Kf[i] * (altitude_gps - constante);
207 I[0][0] = Kf[0];
I[0][1] = 0;
I[0][2] = 0;
208 I[1][0] = Kf[1];
I[1][1] = 0;
I[1][2] = 0;
209 I[2][0] = Kf[2];
I[2][1] = 0;
I[2][2] = 0;
211 for (i = 0; i < 3; i++) {
212 for (j = 0; j < 3; j++) {
213 k->
P[i][j] = k->
P[i][j] -
I[i][0] * k->
P[0][j] -
I[i][1] * k->
P[1][j] -
I[i][2] * k->
P[2][j];
232 float Kf[3] = { 0., 0., 0. };
239 if (fabs(div) > 1e-5) {
241 Kf[0] = (k->
P[0][0] + k->
P[0][2]) / div;
242 Kf[1] = (k->
P[1][0] + k->
P[1][2]) / div;
243 Kf[2] = (k->
P[2][0] + k->
P[2][2]) / div;
247 float constante = k->
X[0] + k->
X[2];
248 for (i = 0; i < 3; i++) {
249 k->
X[i] = k->
X[i] + Kf[i] * (altitude_altimetre - constante);
254 I[0][0] = Kf[0];
I[0][1] = 0;
I[0][2] = Kf[0];
255 I[1][0] = Kf[1];
I[1][1] = 0;
I[1][2] = Kf[1];
256 I[2][0] = Kf[2];
I[2][1] = 0;
I[2][2] = Kf[2];
258 for (i = 0; i < 3; i++) {
259 for (j = 0; j < 3; j++) {
260 k->
P[i][j] = k->
P[i][j] -
I[i][0] * k->
P[0][j] -
I[i][1] * k->
P[1][j] -
I[i][2] * k->
P[2][j];
float MarcheAleaBiaisAltimetre
void kalmanEstimation(TypeKalman *k, float accVert)
void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre)
float MarcheAleaAccelerometre
void alt_filter_init(void)
void alt_filter_periodic(void)
void kalmanInit(TypeKalman *k)
void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps)
float P[KALT_N_ETAT][KALT_N_ETAT]
float Md[KALT_N_ETAT][KALT_N_ETAT - 1]
float Ad[KALT_N_ETAT][KALT_N_ETAT]
float W[KALT_N_ETAT - 1][KALT_N_ETAT - 1]
Driver for the EagleTree Systems Altitude Sensor.
Common code for AP and FBW telemetry.
struct GpsState gps
global GPS state
Device independent GPS code (interface).
arch independent UART (Universal Asynchronous Receiver/Transmitter) API