Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
vf_float.c File Reference

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 More...
 
#define VFF_ACCEL_NOISE   0.5
 process noise covariance Q More...
 
#define VFF_MEAS_NOISE   1.0
 measurement noise covariance R More...
 
#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. More...
 
static void update_z_conf (float z_meas, float conf)
 Update altitude. More...
 
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, 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, 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, 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:

◆ vff_init()

void vff_init ( float  init_z,
float  init_zdot,
float  init_bias 
)

Definition at line 71 of file vf_float.c.

References Vff::bias, DefaultPeriodic, Vff::P, register_periodic_telemetry(), send_vff(), vff, VFF_INIT_PXX, VFF_STATE_SIZE, Vff::z, and Vff::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 66 of file vf_float.c.

References vff_init().

+ Here is the call 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 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 108 of file vf_float.c.

References Vff::bias, Vff::P, Qbiasbias, vff, VFF_ACCEL_NOISE, Vff::z, Vff::zdot, and Vff::zdotdot.

◆ vff_realign()

void vff_realign ( float  z_meas)

Definition at line 256 of file vf_float.c.

References vff, Vff::z, Vff::z_meas, and Vff::zdot.

◆ vff_update()

void vff_update ( float  z_meas)

Definition at line 189 of file vf_float.c.

References update_z_conf(), VFF_MEAS_NOISE, and Vff::z_meas.

Referenced by baro_cb().

+ 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 249 of file vf_float.c.

References update_vz_conf().

+ 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 194 of file vf_float.c.

References update_z_conf(), and Vff::z_meas.

+ Here is the call graph for this function:

Variable Documentation

◆ vff

struct Vff vff

Definition at line 1 of file vf_float.c.

Referenced by send_vff(), update_vz_conf(), update_z_conf(), vff_init(), vff_propagate(), and vff_realign().