ekf_range.h File Reference

#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"

Include dependency graph for ekf_range.h:

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

Go to the source code of this file.

Data Structures
struct	EKFRange
	EKF_range structure. More...

Macros
#define	EKF_RANGE_DIM   6

Functions
void	ekf_range_init (struct EKFRange *ekf_range, float P0_pos, float P0_speed, float Q_sigma2, float R_dist, float R_speed, float dt)
	Init EKF_range internal struct. More...
void	ekf_range_set_state (struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed)
	Set initial state vector. More...
void	ekf_range_get_state (struct EKFRange *ekf_range, struct EnuCoor_f *pos, struct EnuCoor_f *speed)
	Get current state. More...
struct EnuCoor_f	ekf_range_get_pos (struct EKFRange *ekf_range)
	Get current pos. More...
struct EnuCoor_f	ekf_range_get_speed (struct EKFRange *ekf_range)
	Get current speed. More...
void	ekf_range_update_noise (struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed)
	Update process and measurement noises. More...
void	ekf_range_predict (struct EKFRange *ekf_range)
	Prediction step. More...
void	ekf_range_update_dist (struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
	Update step based on each new distance data. More...
void	ekf_range_update_speed (struct EKFRange *ekf_range, float speed, uint8_t type)
	Update step based on speed measure. More...

Detailed Description

Author

Gautier Hattenberger EKF based on range measurements algorithm

Definition in file ekf_range.h.

---

Data Structure Documentation

EKFRange

struct EKFRange

EKF_range structure.

state vector: X = [ x xd y yd z zd ]' command vector: U = 0 (constant velocity model) dynamic model: basic kinematic model x_k+1 = x_k + xd_k * dt measures: distance between (fixed and known) anchors and UAV

Definition at line 43 of file ekf_range.h.

Data Fields
float	dt	prediction step (in seconds)
float	P[EKF_RANGE_DIM][EKF_RANGE_DIM]	covariance matrix
float	Q[EKF_RANGE_DIM][EKF_RANGE_DIM]	process noise matrix
float	R_dist	measurement noise on distances (assumed the same for all anchors)
float	R_speed	measurement noise on speed (assumed the same for all axis)
float	state[EKF_RANGE_DIM]	state vector

Macro Definition Documentation

EKF_RANGE_DIM

#define EKF_RANGE_DIM   6

Definition at line 33 of file ekf_range.h.

Function Documentation

ekf_range_get_pos()

struct EnuCoor_f ekf_range_get_pos

(

struct EKFRange *

ekf_range

)

Get current pos.

Parameters

[in]

ekf_range

EKFRange structure

Returns

current position

Definition at line 65 of file ekf_range.c.

References EKFRange::state, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by check_and_compute_data(), and dw1000_arduino_periodic().

Here is the caller graph for this function:

ekf_range_get_speed()

struct EnuCoor_f ekf_range_get_speed

(

struct EKFRange *

ekf_range

)

Get current speed.

Parameters

[in]

ekf_range

EKFRange structure

Returns

current speed

Definition at line 65 of file ekf_range.c.

Referenced by check_and_compute_data(), and dw1000_arduino_periodic().

Here is the caller graph for this function:

ekf_range_get_state()

void ekf_range_get_state	(	struct EKFRange *	ekf_range,
		struct EnuCoor_f *	pos,
		struct EnuCoor_f *	speed
	)

Get current state.

Parameters

[in]	ekf_range	EKFRange structure
[out]	pos	current position
[out]	speed	current speed

Definition at line 65 of file ekf_range.c.

ekf_range_init()

void ekf_range_init	(	struct EKFRange *	ekf_range,
		float	P0_pos,
		float	P0_speed,
		float	Q_sigma2,
		float	R_dist,
		float	R_speed,
		float	dt
	)

Init EKF_range internal struct.

Parameters

[in]	ekf_range	EKFRange structure
[in]	P0_pos	initial covariance on position
[in]	P0_speed	initial covariance on speed
[in]	Q_sigma2	process noise
[in]	R_dist	measurement noise on distance
[in]	R_speed	measurement noise on speed
[in]	dt	prediction time step in seconds

Definition at line 29 of file ekf_range.c.

References EKFRange::dt, EKF_RANGE_DIM, EKFRange::P, EKFRange::Q, EKFRange::R_dist, EKFRange::R_speed, and EKFRange::state.

Referenced by dw1000_arduino_init().

Here is the caller graph for this function:

ekf_range_predict()

void ekf_range_predict

(

struct EKFRange *

ekf_range

)

Prediction step.

Parameters

[in]

ekf_range

EKFRange structure

Prediction step.

F = [ 1 dt 0 0 0 0 0 1 0 0 0 0 0 0 1 dt 0 0 0 0 0 1 0 0 0 0 0 0 1 dt 0 0 0 0 0 1 ]

Definition at line 119 of file ekf_range.c.

References EKFRange::dt, EKF_RANGE_DIM, EKFRange::P, EKFRange::Q, and EKFRange::state.

Referenced by dw1000_arduino_periodic().

Here is the caller graph for this function:

ekf_range_set_state()

void ekf_range_set_state	(	struct EKFRange *	ekf_range,
		struct EnuCoor_f	pos,
		struct EnuCoor_f	speed
	)

Set initial state vector.

This function should be called after initialization of the ekf struct and before running the filter for better results and faster convergence

Parameters

[in]	ekf_range	EKFRange structure
[in]	pos	initial position
[in]	speed	initial speed

Definition at line 55 of file ekf_range.c.

References EKFRange::state, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by check_and_compute_data().

Here is the caller graph for this function:

ekf_range_update_dist()

void ekf_range_update_dist	(	struct EKFRange *	ekf_range,
		float	dist,
		struct EnuCoor_f	anchor
	)

Update step based on each new distance data.

Parameters

[in]	ekf_range	EKFRange structure
[in]	dist	new distance measurement
[in]	anchor	position of the anchor from which the distance is measured

Update step based on each new distance data.

K = PHt(HPHt+R)^1 X = X + K(z-h(X)) P = (I-KH)P

Definition at line 143 of file ekf_range.c.

References if(), K, EKFRange::P, EKFRange::R_dist, EKFRange::state, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.

Referenced by check_and_compute_data().

Here is the call graph for this function:

Here is the caller graph for this function:

ekf_range_update_noise()

void ekf_range_update_noise	(	struct EKFRange *	ekf_range,
		float	Q_sigma2,
		float	R_dist,
		float	R_speed
	)

Update process and measurement noises.

Parameters

[in]	ekf_range	EKFRange structure
[in]	Q_sigma2	process noise
[in]	R_dist	measurement noise on distance
[in]	R_speed	measurement noise on speed

Definition at line 93 of file ekf_range.c.

References EKFRange::dt, EKF_RANGE_DIM, EKFRange::Q, EKFRange::R_dist, and EKFRange::R_speed.

Referenced by dw1000_arduino_update_ekf_q(), dw1000_arduino_update_ekf_r_dist(), and dw1000_arduino_update_ekf_r_speed().

Here is the caller graph for this function:

ekf_range_update_speed()

void ekf_range_update_speed	(	struct EKFRange *	ekf_range,
		float	speed,
		uint8_t	type
	)

Update step based on speed measure.

Parameters

[in]	ekf_range	EKFRange structure
[in]	speed	new speed measurement
[in]	type	1: horizontal ground speed norm, 2: vertical ground speed norm, 3: 3D ground speed norm

Definition at line 191 of file ekf_range.c.