vf_extended_float.c File Reference

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

#include "modules/ins/vf_extended_float.h"
#include "generated/airframe.h"
#include "std.h"
#include "modules/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

DEBUG_VFF_EXTENDED

#define DEBUG_VFF_EXTENDED   0

Definition at line 39 of file vf_extended_float.c.

Qbiasbias

#define Qbiasbias   1e-6

Definition at line 65 of file vf_extended_float.c.

Qobsobs

#define Qobsobs   0.f

Definition at line 83 of file vf_extended_float.c.

Qoffoff

#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.

R_ALT

#define R_ALT   0.2f

Definition at line 86 of file vf_extended_float.c.

R_OBS_HEIGHT

#define R_OBS_HEIGHT   8.f

Definition at line 87 of file vf_extended_float.c.

VFF_EXTENDED_ACCEL_NOISE

#define VFF_EXTENDED_ACCEL_NOISE   0.5f

process noise covariance Q

Definition at line 57 of file vf_extended_float.c.

VFF_EXTENDED_INIT_PXX

#define VFF_EXTENDED_INIT_PXX   1.0f

initial covariance diagonal

Definition at line 52 of file vf_extended_float.c.

VFF_EXTENDED_R_BARO

#define VFF_EXTENDED_R_BARO   2.f

Barometer confidence.

Definition at line 62 of file vf_extended_float.c.

Function Documentation

send_vffe()

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, dev, 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:

update_alt_conf()

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:

update_biased_z_conf()

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:

update_obs_height()

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:

update_vz_conf()

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:

vff_init()

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

Here is the call graph for this function:

Here is the caller graph for this function:

vff_init_zero()

void vff_init_zero

(

void

)

Definition at line 107 of file vf_extended_float.c.

References vff_init().

Referenced by ins_int_init().

Here is the call graph for this function:

Here is the caller graph for this function:

vff_propagate()

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.

References VffExtended::accel_noise, VffExtended::bias, DefaultChannel, DefaultDevice, VffExtended::P, Qbiasbias, Qobsobs, Qoffoff, send_vffe(), vff, VffExtended::z, VffExtended::zdot, and VffExtended::zdotdot.

Referenced by ins_int_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

vff_realign()

void vff_realign

(

float

z_meas

)

Definition at line 384 of file vf_extended_float.c.

References VffExtended::obs_height, VffExtended::offset, vff, VffExtended::z, VffExtended::z_meas, and VffExtended::zdot.

Referenced by baro_cb().

Here is the caller graph for this function:

vff_update_agl()

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.

Here is the call graph for this function:

vff_update_baro()

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

Here is the call graph for this function:

Here is the caller graph for this function:

vff_update_baro_conf()

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

Here is the call graph for this function:

Here is the caller graph for this function:

vff_update_obs_height()

void vff_update_obs_height

(

float

obs_height

)

Definition at line 379 of file vf_extended_float.c.

References VffExtended::obs_height, 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:

vff_update_vz_conf()

void vff_update_vz_conf	(	float	vz_meas,
		float	conf
	)

Definition at line 433 of file vf_extended_float.c.

References update_vz_conf().

Referenced by ins_int_update_gps(), and vel_est_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

vff_update_z()

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.

Here is the call graph for this function:

vff_update_z_conf()

void vff_update_z_conf	(	float	z_meas,
		float	conf
	)

Definition at line 310 of file vf_extended_float.c.

References update_alt_conf(), and VffExtended::z_meas.

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

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

vff

struct VffExtended vff

Definition at line 1 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().