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"

Include dependency graph for vf_float.c:

Go to the source code of this file.

Macros
#define	VFF_INIT_PXX 1.0
	initial error covariance diagonal

#define	VFF_ACCEL_NOISE 0.5
	process noise covariance Q

#define	VFF_MEAS_NOISE 1.0
	measurement noise covariance R

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

static void	update_z_conf (float z_meas, float conf)
	Update altitude.

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

Detailed Description

Vertical filter (in float) estimating altitude, velocity and accel bias.

X = [ z zdot bias ]

Definition in file vf_float.c.

Macro Definition Documentation

◆ Qbiasbias

#define Qbiasbias 1e-7

Definition at line 51 of file vf_float.c.

◆ VFF_ACCEL_NOISE

#define VFF_ACCEL_NOISE 0.5

process noise covariance Q

Definition at line 42 of file vf_float.c.

◆ VFF_INIT_PXX

#define VFF_INIT_PXX 1.0

initial error covariance diagonal

Definition at line 37 of file vf_float.c.

◆ VFF_MEAS_NOISE

#define VFF_MEAS_NOISE 1.0

measurement noise covariance R

Definition at line 47 of file vf_float.c.

Function Documentation

◆ send_vff()

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

static

Definition at line 58 of file vf_float.c.

References Vff::bias, dev, foo, Vff::P, vff, Vff::z, Vff::z_meas, and Vff::zdot.

Referenced by vff_init().

Here is the caller graph for this function:

◆ update_vz_conf()

static void update_vz_conf	(	float	vz,
		float	conf
	)

inlinestatic

Definition at line 215 of file vf_float.c.

References Vff::bias, foo, Vff::P, vff, Vff::z, and Vff::zdot.

Referenced by vff_update_vz_conf().

Here is the caller graph for this function:

◆ update_z_conf()

static void update_z_conf	(	float	z_meas,
		float	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, foo, Vff::P, vff, Vff::z, Vff::z_meas, and Vff::zdot.

Referenced by vff_update(), and vff_update_z_conf().

Here is the caller graph for this function: