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

#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "generated/airframe.h"

Include dependency graph for hf_float.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	HfilterFloat

Macros
#define	HFF_STATE_SIZE 3

Functions
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_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos)
	Update position. More...

void	hff_update_vel (struct FloatVect2 vel, struct FloatVect2 Rvel)

void	hff_realign (struct FloatVect2 pos, struct FloatVect2 vel)

Variables
struct HfilterFloat	hff

Detailed Description

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

Definition in file hf_float.h.

Data Structure Documentation

struct HfilterFloat

Definition at line 40 of file hf_float.h.

Data Fields
uint16_t	lag_counter
bool	rollback
float	x
float	xbias
float	xdot
float	xdotdot
float	xP[HFF_STATE_SIZE][HFF_STATE_SIZE]
float	y
float	ybias
float	ydot
float	ydotdot
float	yP[HFF_STATE_SIZE][HFF_STATE_SIZE]

Macro Definition Documentation

#define HFF_STATE_SIZE 3

Definition at line 38 of file hf_float.h.

Referenced by hff_init_x(), and hff_init_y().

Function Documentation

void hff_init	(	float	init_x,
		float	init_xdot,
		float	init_y,
		float	init_ydot
	)

Definition at line 274 of file hf_float.c.

References AHRS_PROPAGATE_FREQUENCY, DefaultPeriodic, hff, HFF_DT, hff_init_x(), hff_init_y(), hff_lost_counter, HFF_LOST_LIMIT, hff_lost_limit, HFF_LOWPASS_CUTOFF_FREQUENCY, hff_ps_counter, HFF_R_GPS_SPEED, HFF_R_POS, hff_rb_n, hff_speed_lost_counter, init_butterworth_2_low_pass_int(), HfilterFloat::lag_counter, lag_counter_err, past_save_counter, register_periodic_telemetry(), Rgps_pos, Rgps_vel, HfilterFloat::rollback, save_counter, SAVE_DONE, send_hff(), and send_hff_debug().

Referenced by ins_int_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void hff_propagate ( void )

Definition at line 479 of file hf_float.c.

References ACCEL_FLOAT_OF_BFP, hff, HFF_DT, hff_lost_counter, hff_lost_limit, hff_propagate_x(), hff_propagate_y(), hff_ps_counter, hff_rb_n, hff_speed_lost_counter, hff_xdd_meas, hff_ydd_meas, int32_rmat_transp_vmult(), HfilterFloat::lag_counter, PRINT_DBG, HfilterFloat::rollback, save_counter, stateGetAccelBody_i(), stateGetNedToBodyRMat_i(), update_butterworth_2_low_pass_int(), Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by ins_int_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

void hff_realign	(	struct FloatVect2	pos,
		struct FloatVect2	vel
	)

Definition at line 602 of file hf_float.c.

References hff, hff_rb_n, past_save_counter, save_counter, SAVE_DONE, HfilterFloat::x, FloatVect2::x, HfilterFloat::xbias, HfilterFloat::xdot, HfilterFloat::y, FloatVect2::y, HfilterFloat::ybias, and HfilterFloat::ydot.

Referenced by ins_int_update_gps().

Here is the caller graph for this function:

void hff_update_gps	(	struct FloatVect2 *	pos_ned,
		struct FloatVect2 *	speed_ned
	)

Definition at line 541 of file hf_float.c.

References gps, hff, hff_lost_counter, HFF_R_GPS_SPEED_MIN, HFF_R_POS_MIN, hff_rb_n, hff_update_gps(), hff_update_x(), hff_update_xdot(), hff_update_y(), hff_update_ydot(), HfilterFloat::lag_counter, lag_counter_err, GpsState::pacc, past_save_counter, PRINT_DBG, Rgps_pos, Rgps_vel, HfilterFloat::rollback, GpsState::sacc, save_counter, HfilterFloat::x, FloatVect2::x, HfilterFloat::y, and FloatVect2::y.

Referenced by hff_update_gps(), and ins_int_update_gps().

Here is the call graph for this function:

Here is the caller graph for this function:

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: