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

Horizontal filter (x,y) to estimate position and velocity. More...

#include "subsystems/ins/hf_float.h"
#include "subsystems/imu.h"
#include "state.h"
#include "subsystems/gps.h"
#include <stdlib.h>
#include "filters/low_pass_filter.h"
#include "generated/airframe.h"
#include "subsystems/datalink/telemetry.h"
+ Include dependency graph for hf_float.c:

Go to the source code of this file.

Macros

#define PRINT_DBG(_l, _p)   {}
 
#define AHRS_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY
 
#define HFF_FREQ   (AHRS_PROPAGATE_FREQUENCY / HFF_PRESCALER)
 horizontal filter propagation frequency More...
 
#define HFF_DT   (1./HFF_FREQ)
 
#define HFF_INIT_PXX   1.
 initial covariance diagonal More...
 
#define HFF_ACCEL_NOISE   0.5
 process noise (is the same for x and y) More...
 
#define HFF_Q   HFF_ACCEL_NOISE
 
#define HFF_Qdotdot   HFF_ACCEL_NOISE
 
#define HFF_R_POS   8.
 
#define HFF_R_POS_MIN   3.
 
#define HFF_R_GPS_SPEED   2.
 
#define HFF_R_GPS_SPEED_MIN   0.25
 
#define HFF_UPDATE_GPS_SPEED   TRUE
 
#define HFF_LOWPASS_CUTOFF_FREQUENCY   14
 
#define HFF_Qbiasbias   1e-7
 
#define SAVE_NOW   0
 
#define SAVING   -1
 
#define SAVE_DONE   -2
 
#define HFF_LOST_LIMIT   1000
 

Functions

static void hff_init_x (float init_x, float init_xdot, float init_xbias)
 
static void hff_init_y (float init_y, float init_ydot, float init_ybias)
 
static void hff_propagate_x (struct HfilterFloat *filt, float dt)
 Propagate the filter in time. More...
 
static void hff_propagate_y (struct HfilterFloat *filt, float dt)
 
static void hff_update_x (struct HfilterFloat *filt, float x_meas, float Rpos)
 
static void hff_update_y (struct HfilterFloat *filt, float y_meas, float Rpos)
 
static void hff_update_xdot (struct HfilterFloat *filt, float vel, float Rvel)
 
static void hff_update_ydot (struct HfilterFloat *filt, float vel, float Rvel)
 
static void send_hff (struct transport_tx *trans, struct link_device *dev)
 
static void send_hff_debug (struct transport_tx *trans, struct link_device *dev)
 
void hff_init (float init_x, float init_xdot, float init_y, float init_ydot)
 
void hff_propagate (void)
 
void hff_update_gps (struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
 
void hff_realign (struct FloatVect2 pos, struct FloatVect2 vel)
 
void hff_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos)
 Update position. More...
 
void hff_update_vel (struct FloatVect2 vel, struct FloatVect2 Rvel)
 

Variables

Butterworth2LowPass_int filter_x
 
Butterworth2LowPass_int filter_y
 
Butterworth2LowPass_int filter_z
 
float Rgps_pos
 
float Rgps_vel
 
struct HfilterFloat hff
 
static float hff_xdd_meas = 0
 
static float hff_ydd_meas = 0
 
static float hff_xd_meas = 0
 
static float hff_yd_meas = 0
 
static float hff_x_meas = 0
 
static float hff_y_meas = 0
 
static int hff_ps_counter
 counter for hff propagation More...
 
struct HfilterFloathff_rb_last
 ringbuffer read pointer More...
 
static int hff_rb_n
 ringbuffer fill count More...
 
static int16_t lag_counter_err
 by how many steps the estimated GPS validity point in time differed from GPS_LAG_N More...
 
static int16_t save_counter
 counts down the propagation steps until the filter state is saved again More...
 
static int past_save_counter
 
static uint16_t hff_lost_limit
 
static uint16_t hff_lost_counter
 
static uint16_t hff_speed_lost_counter
 

Detailed Description

Horizontal filter (x,y) to estimate position and velocity.

Definition in file hf_float.c.

Macro Definition Documentation

#define AHRS_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY

Definition at line 55 of file hf_float.c.

Referenced by gyro_cb(), and hff_init().

#define HFF_ACCEL_NOISE   0.5

process noise (is the same for x and y)

Definition at line 78 of file hf_float.c.

#define HFF_DT   (1./HFF_FREQ)

Definition at line 72 of file hf_float.c.

Referenced by hff_init(), and hff_propagate().

#define HFF_FREQ   (AHRS_PROPAGATE_FREQUENCY / HFF_PRESCALER)

horizontal filter propagation frequency

Definition at line 71 of file hf_float.c.

#define HFF_INIT_PXX   1.

initial covariance diagonal

Definition at line 75 of file hf_float.c.

Referenced by hff_init_x(), and hff_init_y().

#define HFF_LOST_LIMIT   1000

Definition at line 209 of file hf_float.c.

Referenced by hff_init().

#define HFF_LOWPASS_CUTOFF_FREQUENCY   14

Definition at line 103 of file hf_float.c.

Referenced by hff_init().

#define HFF_Q   HFF_ACCEL_NOISE

Definition at line 80 of file hf_float.c.

Referenced by hff_propagate_x(), and hff_propagate_y().

#define HFF_Qbiasbias   1e-7

Definition at line 142 of file hf_float.c.

Referenced by hff_propagate_x(), and hff_propagate_y().

#define HFF_Qdotdot   HFF_ACCEL_NOISE

Definition at line 81 of file hf_float.c.

Referenced by hff_propagate_x(), and hff_propagate_y().

#define HFF_R_GPS_SPEED   2.

Definition at line 92 of file hf_float.c.

Referenced by hff_init().

#define HFF_R_GPS_SPEED_MIN   0.25

Definition at line 95 of file hf_float.c.

Referenced by hff_update_gps().

#define HFF_R_POS   8.

Definition at line 85 of file hf_float.c.

Referenced by hff_init().

#define HFF_R_POS_MIN   3.

Definition at line 88 of file hf_float.c.

Referenced by hff_update_gps().

#define HFF_UPDATE_GPS_SPEED   TRUE

Definition at line 99 of file hf_float.c.

#define PRINT_DBG (   _l,
  _p 
)    {}

Definition at line 50 of file hf_float.c.

Referenced by hff_propagate(), and hff_update_gps().

#define SAVE_DONE   -2

Definition at line 207 of file hf_float.c.

Referenced by hff_init(), and hff_realign().

#define SAVE_NOW   0

Definition at line 205 of file hf_float.c.

#define SAVING   -1

Definition at line 206 of file hf_float.c.

Function Documentation

static void hff_init_x ( float  init_x,
float  init_xdot,
float  init_xbias 
)
static

Definition at line 324 of file hf_float.c.

References hff, HFF_INIT_PXX, HFF_STATE_SIZE, HfilterFloat::x, HfilterFloat::xbias, HfilterFloat::xdot, and HfilterFloat::xP.

Referenced by hff_init().

+ Here is the caller graph for this function:

static void hff_init_y ( float  init_y,
float  init_ydot,
float  init_ybias 
)
static

Definition at line 338 of file hf_float.c.

References hff, HFF_INIT_PXX, HFF_STATE_SIZE, HfilterFloat::y, HfilterFloat::ybias, HfilterFloat::ydot, and HfilterFloat::yP.

Referenced by hff_init().

+ Here is the caller graph for this function:

static void hff_propagate_x ( struct HfilterFloat filt,
float  dt 
)
static

Propagate the filter in time.

F = [ 1 dt -dt^2/2 0 1 -dt 0 0 1 ];

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

Q = [ HFF_Q 0 0 0 HFF_Qdotdot 0 0 0 HFF_Qbiasbias ];

Xk1 = F * Xk0 + B * accel;

Pk1 = F * Pk0 * F' + Q;

Definition at line 638 of file hf_float.c.

References HFF_Q, HFF_Qbiasbias, HFF_Qdotdot, hff_xdd_meas, HfilterFloat::x, HfilterFloat::xbias, HfilterFloat::xdot, HfilterFloat::xdotdot, and HfilterFloat::xP.

Referenced by hff_propagate().

+ Here is the caller graph for this function:

static void hff_propagate_y ( struct HfilterFloat filt,
float  dt 
)
static

Definition at line 666 of file hf_float.c.

References HFF_Q, HFF_Qbiasbias, HFF_Qdotdot, hff_ydd_meas, HfilterFloat::y, HfilterFloat::ybias, HfilterFloat::ydot, HfilterFloat::ydotdot, and HfilterFloat::yP.

Referenced by hff_propagate().

+ Here is the caller graph for this function:

void hff_realign ( struct FloatVect2  pos,
struct FloatVect2  vel 
)
void hff_update_gps ( struct FloatVect2 pos_ned,
struct FloatVect2 speed_ned 
)
void hff_update_pos ( struct FloatVect2  pos,
struct FloatVect2  Rpos 
)

Update position.

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 711 of file hf_float.c.

References hff, hff_lost_counter, hff_update_x(), hff_update_y(), FloatVect2::x, and FloatVect2::y.

Referenced by pos_est_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void hff_update_vel ( struct FloatVect2  vel,
struct FloatVect2  Rvel 
)

Definition at line 806 of file hf_float.c.

References hff, hff_speed_lost_counter, hff_update_xdot(), hff_update_ydot(), FloatVect2::x, and FloatVect2::y.

Referenced by vel_est_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void hff_update_x ( struct HfilterFloat filt,
float  x_meas,
float  Rpos 
)
static

Definition at line 718 of file hf_float.c.

References hff_x_meas, HfilterFloat::x, HfilterFloat::xbias, HfilterFloat::xdot, and HfilterFloat::xP.

Referenced by hff_update_gps(), and hff_update_pos().

+ Here is the caller graph for this function:

static void hff_update_xdot ( struct HfilterFloat filt,
float  vel,
float  Rvel 
)
static

Definition at line 820 of file hf_float.c.

References hff_xd_meas, HfilterFloat::x, HfilterFloat::xbias, HfilterFloat::xdot, and HfilterFloat::xP.

Referenced by hff_update_gps(), and hff_update_vel().

+ Here is the caller graph for this function:

static void hff_update_y ( struct HfilterFloat filt,
float  y_meas,
float  Rpos 
)
static

Definition at line 753 of file hf_float.c.

References hff_y_meas, HfilterFloat::y, HfilterFloat::ybias, HfilterFloat::ydot, and HfilterFloat::yP.

Referenced by hff_update_gps(), and hff_update_pos().

+ Here is the caller graph for this function:

static void hff_update_ydot ( struct HfilterFloat filt,
float  vel,
float  Rvel 
)
static

Definition at line 855 of file hf_float.c.

References hff_yd_meas, HfilterFloat::y, HfilterFloat::ybias, HfilterFloat::ydot, and HfilterFloat::yP.

Referenced by hff_update_gps(), and hff_update_vel().

+ Here is the caller graph for this function:

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

Definition at line 234 of file hf_float.c.

References hff, HfilterFloat::x, HfilterFloat::xbias, HfilterFloat::xdot, HfilterFloat::xdotdot, HfilterFloat::y, HfilterFloat::ybias, HfilterFloat::ydot, and HfilterFloat::ydotdot.

Referenced by hff_init().

+ Here is the caller graph for this function:

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

Definition at line 247 of file hf_float.c.

References hff, hff_x_meas, hff_xd_meas, hff_y_meas, hff_yd_meas, HfilterFloat::xP, and HfilterFloat::yP.

Referenced by hff_init().

+ Here is the caller graph for this function:

Variable Documentation

Definition at line 111 of file hf_float.c.

Definition at line 112 of file hf_float.c.

Definition at line 113 of file hf_float.c.

uint16_t hff_lost_counter
static

Definition at line 211 of file hf_float.c.

Referenced by hff_init(), hff_propagate(), hff_update_gps(), and hff_update_pos().

uint16_t hff_lost_limit
static

Definition at line 210 of file hf_float.c.

Referenced by hff_init(), and hff_propagate().

int hff_ps_counter
static

counter for hff propagation

Definition at line 139 of file hf_float.c.

Referenced by hff_init(), and hff_propagate().

struct HfilterFloat* hff_rb_last

ringbuffer read pointer

Definition at line 195 of file hf_float.c.

int hff_rb_n
static

ringbuffer fill count

Definition at line 196 of file hf_float.c.

Referenced by hff_init(), hff_propagate(), hff_realign(), and hff_update_gps().

uint16_t hff_speed_lost_counter
static

Definition at line 211 of file hf_float.c.

Referenced by hff_init(), hff_propagate(), and hff_update_vel().

float hff_x_meas = 0
static

Definition at line 135 of file hf_float.c.

Referenced by hff_update_x(), and send_hff_debug().

float hff_xd_meas = 0
static

Definition at line 131 of file hf_float.c.

Referenced by hff_update_xdot(), and send_hff_debug().

float hff_xdd_meas = 0
static

Definition at line 127 of file hf_float.c.

Referenced by hff_propagate(), and hff_propagate_x().

float hff_y_meas = 0
static

Definition at line 136 of file hf_float.c.

Referenced by hff_update_y(), and send_hff_debug().

float hff_yd_meas = 0
static

Definition at line 132 of file hf_float.c.

Referenced by hff_update_ydot(), and send_hff_debug().

float hff_ydd_meas = 0
static

Definition at line 128 of file hf_float.c.

Referenced by hff_propagate(), and hff_propagate_y().

int16_t lag_counter_err
static

by how many steps the estimated GPS validity point in time differed from GPS_LAG_N

Definition at line 200 of file hf_float.c.

Referenced by hff_init(), and hff_update_gps().

int past_save_counter
static

Definition at line 204 of file hf_float.c.

Referenced by hff_init(), hff_realign(), and hff_update_gps().

float Rgps_pos

Definition at line 116 of file hf_float.c.

Referenced by hff_init(), and hff_update_gps().

float Rgps_vel

Definition at line 116 of file hf_float.c.

Referenced by hff_init(), and hff_update_gps().

int16_t save_counter
static

counts down the propagation steps until the filter state is saved again

Definition at line 203 of file hf_float.c.

Referenced by hff_init(), hff_propagate(), hff_realign(), and hff_update_gps().