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 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.0
	initial covariance diagonal More...
#define	VFF_EXTENDED_ACCEL_NOISE   0.5
	process noise covariance Q More...
#define	Qbiasbias   1e-7
#define	Qoffoff   1e-4
#define	R_BARO   1.
#define	R_ALT   0.1
#define	R_OFFSET   1.

Functions
void	vff_init_zero (void)
void	vff_init (float init_z, float init_zdot, float init_accel_bias, float init_baro_offset)
void	vff_propagate (float accel, float dt)
	Propagate the filter in time. More...
static void	update_baro_conf (float z_meas, float conf)
	Update sensor "with" offset (baro). More...
void	vff_update_baro (float z_meas)
void	vff_update_baro_conf (float z_meas, float conf)
static void	update_alt_conf (float z_meas, float conf)
	Update sensor "without" offset (gps, sonar) H = [1 0 0 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;. More...
void	vff_update_z (float z_meas)
void	vff_update_z_conf (float z_meas, float conf)
static void	update_offset_conf (float offset, float conf)
	Update sensor offset (baro). More...
void	vff_update_offset (float offset)
void	vff_realign (float z_meas)

Variables
struct VffExtended	vff

Detailed Description

Extended vertical filter (in float).

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

X = [ z zdot accel_bias baro_offset ]

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

Definition at line 60 of file vf_extended_float.c.

Referenced by vff_propagate().

#define Qoffoff   1e-4

Definition at line 61 of file vf_extended_float.c.

Referenced by vff_propagate().

#define R_ALT   0.1

Definition at line 63 of file vf_extended_float.c.

Referenced by vff_update_z().

#define R_BARO   1.

Definition at line 62 of file vf_extended_float.c.

Referenced by vff_update_baro().

#define R_OFFSET   1.

Definition at line 64 of file vf_extended_float.c.

Referenced by vff_update_offset().

#define VFF_EXTENDED_ACCEL_NOISE   0.5

process noise covariance Q

Definition at line 57 of file vf_extended_float.c.

Referenced by vff_propagate().

#define VFF_EXTENDED_INIT_PXX   1.0

initial covariance diagonal

Definition at line 52 of file vf_extended_float.c.

Referenced by vff_init().

Function Documentation

static void update_alt_conf ( float  z_meas, float  conf  )

static

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

References VffExtended::bias, K0, VffExtended::offset, VffExtended::P, vff, VffExtended::z, VffExtended::z_meas, and VffExtended::zdot.

Referenced by vff_update_z(), and vff_update_z_conf().

Here is the caller graph for this function:

static void update_baro_conf ( float  z_meas, float  conf  )

static

Update sensor "with" offset (baro).

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

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

Referenced by vff_update_baro(), and vff_update_baro_conf().

Here is the caller graph for this function:

static void update_offset_conf ( float  offset, float  conf  )

static

Update sensor offset (baro).

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

References VffExtended::bias, K0, VffExtended::offset, VffExtended::P, vff, VffExtended::z, and VffExtended::zdot.

Referenced by vff_update_offset().

Here is the caller graph for this function:

void vff_init	(	float	init_z,
		float	init_zdot,
		float	init_accel_bias,
		float	init_baro_offset
	)

Definition at line 85 of file vf_extended_float.c.

References VffExtended::bias, DefaultPeriodic, VffExtended::offset, VffExtended::P, register_periodic_telemetry(), vff, VFF_EXTENDED_INIT_PXX, VFF_STATE_SIZE, VffExtended::z, and VffExtended::zdot.

Referenced by vff_init_zero(), and vff_realign().

Here is the call graph for this function:

Here is the caller graph for this function:

void vff_init_zero

(

void

)

Definition at line 80 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 1 -dt 0 0 0 1 0 0 0 0 1 ];

B = [ dt^2/2 dt 0 0]';

Q = [ Qzz 0 0 0 0 Qzdotzdot 0 0 0 0 Qbiasbias 0 0 0 0 0 Qoffoff ];

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 127 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 347 of file vf_extended_float.c.

Referenced by baro_cb().

Here is the caller graph for this function:

void vff_update_baro

(

float

z_meas

)

Definition at line 216 of file vf_extended_float.c.

References R_BARO, and 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 221 of file vf_extended_float.c.

References update_baro_conf().

Here is the call graph for this function:

void vff_update_offset

(

float

offset

)

Definition at line 341 of file vf_extended_float.c.

References R_OFFSET, and update_offset_conf().

Here is the call graph for this function:

void vff_update_z

(

float

z_meas

)

Definition at line 279 of file vf_extended_float.c.

References R_ALT, and update_alt_conf().

Here is the call graph for this function:

void vff_update_z_conf	(	float	z_meas,
		float	conf
	)

Definition at line 284 of file vf_extended_float.c.

Referenced by ins_int_update_gps().

Here is the caller graph for this function:

Variable Documentation

struct VffExtended vff

Definition at line 66 of file vf_extended_float.c.

Referenced by ins_update_from_vff(), update_alt_conf(), update_baro_conf(), update_offset_conf(), vff_init(), and vff_propagate().