Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Vertical filter (in float) estimating altitude, velocity and accel bias. More...
#include "modules/ins/vf_float.h"
#include "generated/airframe.h"
#include "std.h"
#include "modules/datalink/telemetry.h"
Go to the source code of this file.
Macros | |
#define | VFF_INIT_PXX 1.0 |
initial error covariance diagonal More... | |
#define | VFF_ACCEL_NOISE 0.5 |
process noise covariance Q More... | |
#define | VFF_MEAS_NOISE 1.0 |
measurement noise covariance R More... | |
#define | Qbiasbias 1e-7 |
Functions | |
static void | send_vff (struct transport_tx *trans, struct link_device *dev) |
void | vff_init_zero (void) |
void | vff_init (float init_z, float init_zdot, float init_bias) |
void | vff_propagate (float accel, float dt) |
Propagate the filter in time. More... | |
static void | update_z_conf (float z_meas, float conf) |
Update altitude. More... | |
void | vff_update (float z_meas) |
void | vff_update_z_conf (float z_meas, float conf) |
static void | update_vz_conf (float vz, float conf) |
void | vff_update_vz_conf (float vz_meas, float conf) |
void | vff_realign (float z_meas) |
Variables | |
struct Vff | vff |
Vertical filter (in float) estimating altitude, velocity and accel bias.
X = [ z zdot bias ]
Definition in file vf_float.c.
#define Qbiasbias 1e-7 |
Definition at line 51 of file vf_float.c.
#define VFF_ACCEL_NOISE 0.5 |
process noise covariance Q
Definition at line 42 of file vf_float.c.
#define VFF_INIT_PXX 1.0 |
initial error covariance diagonal
Definition at line 37 of file vf_float.c.
#define VFF_MEAS_NOISE 1.0 |
measurement noise covariance R
Definition at line 47 of file vf_float.c.
|
static |
Definition at line 58 of file vf_float.c.
References Vff::bias, dev, Vff::P, vff, Vff::z, Vff::z_meas, and Vff::zdot.
Referenced by vff_init().
|
inlinestatic |
Definition at line 215 of file vf_float.c.
References Vff::bias, Vff::P, vff, Vff::z, and Vff::zdot.
Referenced by vff_update_vz_conf().
|
inlinestatic |
Update altitude.
H = [1 0 0]; R = 0.1; // state residual y = rangemeter - H * Xm; // covariance residual S = H*Pm*H' + R; // kalman gain K = Pm*H'*inv(S); // update state Xp = Xm + K*y; // update covariance Pp = Pm - K*H*Pm;
Definition at line 153 of file vf_float.c.
References Vff::bias, Vff::P, vff, Vff::z, Vff::z_meas, and Vff::zdot.
Referenced by vff_update(), and vff_update_z_conf().
void vff_init | ( | float | init_z, |
float | init_zdot, | ||
float | init_bias | ||
) |
Definition at line 71 of file vf_float.c.
References Vff::bias, DefaultPeriodic, Vff::P, register_periodic_telemetry(), send_vff(), vff, VFF_INIT_PXX, VFF_STATE_SIZE, Vff::z, and Vff::zdot.
Referenced by vff_init_zero().
void vff_init_zero | ( | void | ) |
Definition at line 66 of file vf_float.c.
References vff_init().
void vff_propagate | ( | float | accel, |
float | dt | ||
) |
Propagate the filter in time.
F = [ 1 dt -dt^2/2 0 1 -dt 0 0 1 ];
B = [ dt^2/2 dt 0]';
Q = [ 0.01 0 0 0 0.01 0 0 0 0.001 ];
Xk1 = F * Xk0 + B * accel;
Pk1 = F * Pk0 * F' + Q;
Definition at line 108 of file vf_float.c.
References Vff::bias, Vff::P, Qbiasbias, vff, VFF_ACCEL_NOISE, Vff::z, Vff::zdot, and Vff::zdotdot.
void vff_realign | ( | float | z_meas | ) |
Definition at line 256 of file vf_float.c.
References vff, Vff::z, Vff::z_meas, and Vff::zdot.
void vff_update | ( | float | z_meas | ) |
Definition at line 189 of file vf_float.c.
References update_z_conf(), VFF_MEAS_NOISE, and Vff::z_meas.
Referenced by baro_cb().
void vff_update_vz_conf | ( | float | vz_meas, |
float | conf | ||
) |
Definition at line 249 of file vf_float.c.
References update_vz_conf().
void vff_update_z_conf | ( | float | z_meas, |
float | conf | ||
) |
Definition at line 194 of file vf_float.c.
References update_z_conf(), and Vff::z_meas.
struct Vff vff |
Definition at line 1 of file vf_float.c.
Referenced by send_vff(), update_vz_conf(), update_z_conf(), vff_init(), vff_propagate(), and vff_realign().