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.
if(GpsFixValid() &&e_identification_started)
struct GpsState gps
global GPS state
Device independent GPS code (interface)
int32_t hmsl
height above mean sea level (MSL) in mm
arch independent UART (Universal Asynchronous Receiver/Transmitter) API