Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
Macros | Functions | Variables
vf_extended_float.c File Reference

Extended vertical filter (in float). More...

#include "subsystems/ins/vf_extended_float.h"
#include "generated/airframe.h"
#include "std.h"
#include "subsystems/datalink/telemetry.h"
+ Include dependency graph for vf_extended_float.c:

Go to the source code of this file.

Macros

#define DEBUG_VFF_EXTENDED   0
 
#define VFF_EXTENDED_INIT_PXX   1.0f
 initial covariance diagonal More...
 
#define VFF_EXTENDED_ACCEL_NOISE   0.5f
 process noise covariance Q More...
 
#define VFF_EXTENDED_R_BARO   2.f
 Barometer confidence. More...
 
#define Qbiasbias   1e-6
 
#define Qoffoff   1e-4f
 VFF_EXTENDED_NON_FLAT_GROUND removes the assumption of a flat ground and tries to estimate the height of the obstacles under the vehicle. More...
 
#define Qobsobs   0.f
 
#define R_ALT   0.2f
 
#define R_OBS_HEIGHT   8.f
 

Functions

void vff_update_obs_height (float obs_height)
 
static void send_vffe (struct transport_tx *trans, struct link_device *dev)
 
void vff_init_zero (void)
 
void vff_init (float init_z, float init_zdot, float init_accel_bias, float init_offset, float init_obs_height)
 
void vff_propagate (float accel, float dt)
 Propagate the filter in time. More...
 
static void update_biased_z_conf (float z_meas, float conf)
 Update sensor "with" offset (baro, sonar). More...
 
static void update_alt_conf (float z_meas, float conf)
 Update sensor "without" offset (gps, sonar) H = [1 0 0 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;. More...
 
void vff_update_z_conf (float z_meas, float conf)
 
void vff_update_z (float z_meas)
 
void vff_update_agl (float z_meas, float conf)
 
void vff_update_baro_conf (float z_meas, float conf)
 
void vff_update_baro (float z_meas)
 
static void update_obs_height (float obs_height, float conf)
 Update obstacle height. More...
 
void vff_realign (float z_meas)
 
static void update_vz_conf (float vz, float conf)
 
void vff_update_vz_conf (float vz_meas, float conf)
 

Variables

struct VffExtended vff
 

Detailed Description

Extended vertical filter (in float).

Estimates altitude, vertical speed, accelerometer bias barometer offset and obstacle height.

X = [ z zdot accel_bias baro_offset obstacle_height ]

Definition in file vf_extended_float.c.

Macro Definition Documentation

#define DEBUG_VFF_EXTENDED   0

Definition at line 39 of file vf_extended_float.c.

#define Qbiasbias   1e-6

Definition at line 65 of file vf_extended_float.c.

Referenced by vff_propagate().

#define Qobsobs   0.f

Definition at line 83 of file vf_extended_float.c.

Referenced by vff_propagate().

#define Qoffoff   1e-4f

VFF_EXTENDED_NON_FLAT_GROUND removes the assumption of a flat ground and tries to estimate the height of the obstacles under the vehicle.

This results in a barometer focused filter with the agl measurement used to slowly update the barometer offset. Over time, the obstacle height will tend towards zero, this will result in a relatively smooth transition over sort obstacles but will maintain a constant height above the true ground. If not defined the filter is agl measurement heavy with the result assuming the agl measurement is truth and is used to quickly update the barometer offset.

Definition at line 82 of file vf_extended_float.c.

Referenced by vff_propagate().

#define R_ALT   0.2f

Definition at line 86 of file vf_extended_float.c.

Referenced by vff_init().

#define R_OBS_HEIGHT   8.f

Definition at line 87 of file vf_extended_float.c.

Referenced by vff_init().

#define VFF_EXTENDED_ACCEL_NOISE   0.5f

process noise covariance Q

Definition at line 57 of file vf_extended_float.c.

Referenced by vff_init().

#define VFF_EXTENDED_INIT_PXX   1.0f

initial covariance diagonal

Definition at line 52 of file vf_extended_float.c.

Referenced by vff_init().

#define VFF_EXTENDED_R_BARO   2.f

Barometer confidence.

Definition at line 62 of file vf_extended_float.c.

Referenced by vff_init().

Function Documentation

static void send_vffe ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 96 of file vf_extended_float.c.

References VffExtended::bias, VffExtended::obs_height, VffExtended::offset, VffExtended::P, vff, VffExtended::z, VffExtended::z_meas, VffExtended::z_meas_baro, VffExtended::zdot, and VffExtended::zdotdot.

Referenced by vff_init(), and vff_propagate().

+ Here is the caller graph for this function:

static void update_alt_conf ( float  z_meas,
float  conf 
)
static

Update sensor "without" offset (gps, sonar) H = [1 0 0 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 281 of file vf_extended_float.c.

References VffExtended::bias, K, VffExtended::obs_height, VffExtended::offset, P, VffExtended::P, vff, VFF_STATE_SIZE, VffExtended::z, VffExtended::z_meas, and VffExtended::zdot.

Referenced by vff_update_z_conf().

+ Here is the caller graph for this function:

static void update_biased_z_conf ( float  z_meas,
float  conf 
)
static

Update sensor "with" offset (baro, sonar).

H = [1 0 0 -1 0]; // 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 236 of file vf_extended_float.c.

References VffExtended::bias, K, VffExtended::obs_height, VffExtended::offset, P, VffExtended::P, vff, VFF_STATE_SIZE, vff_update_obs_height(), VffExtended::z, VffExtended::z_meas, VffExtended::z_meas_baro, and VffExtended::zdot.

Referenced by vff_update_baro_conf().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void update_obs_height ( float  obs_height,
float  conf 
)
static

Update obstacle height.

H = [0 0 0 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 352 of file vf_extended_float.c.

References VffExtended::bias, K, VffExtended::obs_height, VffExtended::offset, P, VffExtended::P, vff, VFF_STATE_SIZE, VffExtended::z, and VffExtended::zdot.

Referenced by vff_update_obs_height().

+ Here is the caller graph for this function:

static void update_vz_conf ( float  vz,
float  conf 
)
inlinestatic

Definition at line 406 of file vf_extended_float.c.

References VffExtended::bias, K, VffExtended::obs_height, VffExtended::offset, P, VffExtended::P, vff, VFF_STATE_SIZE, VffExtended::z, and VffExtended::zdot.

Referenced by vff_update_vz_conf().

+ Here is the caller graph for this function:

void vff_init ( float  init_z,
float  init_zdot,
float  init_accel_bias,
float  init_offset,
float  init_obs_height 
)

Definition at line 127 of file vf_extended_float.c.

References VffExtended::accel_noise, VffExtended::bias, DefaultPeriodic, VffExtended::obs_height, VffExtended::offset, VffExtended::P, VffExtended::r_alt, R_ALT, VffExtended::r_baro, VffExtended::r_obs_height, 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().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void vff_init_zero ( void  )

Definition at line 107 of file vf_extended_float.c.

Referenced by ins_int_init().

+ Here is the caller graph for this function:

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;

Definition at line 181 of file vf_extended_float.c.

Referenced by ins_int_propagate().

+ Here is the caller graph for this function:

void vff_realign ( float  z_meas)

Definition at line 384 of file vf_extended_float.c.

Referenced by baro_cb().

+ Here is the caller graph for this function:

void vff_update_agl ( float  z_meas,
float  conf 
)

Definition at line 321 of file vf_extended_float.c.

References vff_update_z_conf().

+ Here is the call graph for this function:

void vff_update_baro ( float  z_meas)

Definition at line 333 of file vf_extended_float.c.

References VffExtended::r_baro, vff, and vff_update_baro_conf().

Referenced by baro_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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().

Referenced by vff_update_baro().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void vff_update_obs_height ( float  obs_height)

Definition at line 379 of file vf_extended_float.c.

References VffExtended::r_obs_height, update_obs_height(), and vff.

Referenced by update_biased_z_conf().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void vff_update_vz_conf ( float  vz_meas,
float  conf 
)

Definition at line 433 of file vf_extended_float.c.

Referenced by ins_int_update_gps(), and vel_est_cb().

+ Here is the caller graph for this function:

void vff_update_z ( float  z_meas)

Definition at line 316 of file vf_extended_float.c.

References VffExtended::r_alt, vff, and vff_update_z_conf().

+ Here is the call graph for this function:

void vff_update_z_conf ( float  z_meas,
float  conf 
)

Definition at line 310 of file vf_extended_float.c.

Referenced by ins_int_update_gps(), pos_est_cb(), vff_update_agl(), and vff_update_z().

+ Here is the caller graph for this function:

Variable Documentation

struct VffExtended vff

Definition at line 89 of file vf_extended_float.c.

Referenced by ins_update_from_vff(), send_vffe(), update_alt_conf(), update_biased_z_conf(), update_obs_height(), update_vz_conf(), vff_init(), vff_propagate(), vff_realign(), vff_update_baro(), vff_update_obs_height(), and vff_update_z().