ekf_range.c File Reference

#include "ekf_range.h"
#include <math.h>

Include dependency graph for ekf_range.c:

Go to the source code of this file.

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)
	propagate dynamic model More...
void	ekf_range_update_dist (struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
	correction step 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.c.

Function Documentation

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 75 of file ekf_range.c.

References 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:

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 84 of file ekf_range.c.

References 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:

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.

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

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:

void ekf_range_predict

(

struct EKFRange *

ekf_range

)

propagate dynamic model

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:

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:

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

correction step

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 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 caller graph for this function:

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:

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.