Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Interface for extended vertical filter (in float). More...
Go to the source code of this file.
Data Structures | |
struct | VffExtended |
Macros | |
#define | VFF_STATE_SIZE 5 |
Functions | |
void | vff_init_zero (void) |
void | vff_init (float z, float zdot, float accel_bias, float baro_offset, float obstacle_hieght) |
void | vff_propagate (float accel, float dt) |
Propagate the filter in time. More... | |
void | vff_update_baro (float z_meas) |
void | vff_update_z (float z_meas) |
void | vff_update_baro_conf (float z_meas, float conf) |
void | vff_update_z_conf (float z_meas, float conf) |
void | vff_update_vz_conf (float vz_meas, float conf) |
void | vff_realign (float z_meas) |
void | vff_update_agl (float z_meas, float conf) |
Variables | |
struct VffExtended | vff |
Interface for extended vertical filter (in float).
Definition in file vf_extended_float.h.
struct VffExtended |
Definition at line 35 of file vf_extended_float.h.
Data Fields | ||
---|---|---|
float | accel_noise | |
float | bias | accel bias estimate in m/s^2 |
float | obs_height | estimate of height of obstacles under the vehicle |
float | offset | baro offset estimate |
float | P[VFF_STATE_SIZE][VFF_STATE_SIZE] | covariance matrix |
float | r_alt | |
float | r_baro | |
float | r_obs_height | |
float | z | z-position estimate in m (NED, z-down) |
float | z_meas | last z measurement in m |
float | z_meas_baro | last z measurement from baro in m |
float | zdot | z-velocity estimate in m/s (NED, z-down) |
float | zdotdot | z-acceleration in m/s^2 (NED, z-down) |
#define VFF_STATE_SIZE 5 |
Definition at line 33 of file vf_extended_float.h.
void vff_init | ( | float | z, |
float | zdot, | ||
float | accel_bias, | ||
float | baro_offset, | ||
float | obstacle_hieght | ||
) |
Definition at line 127 of file vf_extended_float.c.
References VffExtended::accel_noise, VffExtended::bias, DefaultPeriodic, VffExtended::obs_height, VffExtended::offset, VffExtended::P, R_ALT, VffExtended::r_alt, VffExtended::r_baro, R_OBS_HEIGHT, VffExtended::r_obs_height, register_periodic_telemetry(), send_vffe(), vff, VFF_EXTENDED_ACCEL_NOISE, VFF_EXTENDED_INIT_PXX, VFF_EXTENDED_R_BARO, VFF_STATE_SIZE, VffExtended::z, and VffExtended::zdot.
Referenced by vff_init_zero().
void vff_init_zero | ( | void | ) |
Definition at line 107 of file vf_extended_float.c.
void vff_propagate | ( | float | accel, |
float | dt | ||
) |
Propagate the filter in time.
F = [ 1 dt -dt^2/2 0 0 0 1 -dt 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 ];
B = [ dt^2/2 dt 0 0 0]';
Q = [ Qzz 0 0 0 0 0 Qzdotzdot 0 0 0 0 0 Qbiasbias 0 0 0 0 0 0 Qoffoff 0 0 0 0 0 0 Qobsobs ];
Qzz = VFF_EXTENDED_ACCEL_NOISE * DT_VFILTER * DT_VFILTER / 2. Qzdotzdot = VFF_EXTENDED_ACCEL_NOISE * DT_VFILTER
Xk1 = F * Xk0 + B * accel;
Pk1 = F * Pk0 * F' + Q;
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 181 of file vf_extended_float.c.
void vff_realign | ( | float | z_meas | ) |
Definition at line 384 of file vf_extended_float.c.
void vff_update_agl | ( | float | z_meas, |
float | conf | ||
) |
Definition at line 321 of file vf_extended_float.c.
References vff_update_z_conf(), and VffExtended::z_meas.
void vff_update_baro | ( | float | z_meas | ) |
Definition at line 333 of file vf_extended_float.c.
References VffExtended::r_baro, vff, vff_update_baro_conf(), and VffExtended::z_meas.
Referenced by baro_cb().
void vff_update_baro_conf | ( | float | z_meas, |
float | conf | ||
) |
Definition at line 327 of file vf_extended_float.c.
References update_biased_z_conf(), and VffExtended::z_meas.
Referenced by vff_update_baro().
void vff_update_vz_conf | ( | float | vz_meas, |
float | conf | ||
) |
Definition at line 433 of file vf_extended_float.c.
void vff_update_z | ( | float | z_meas | ) |
Definition at line 316 of file vf_extended_float.c.
References VffExtended::r_alt, vff, vff_update_z_conf(), and VffExtended::z_meas.
void vff_update_z_conf | ( | float | z_meas, |
float | conf | ||
) |
Definition at line 310 of file vf_extended_float.c.
|
extern |
Definition at line 1 of file vf_extended_float.c.