simple_kinematic_kalman.h File Reference

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

Include dependency graph for simple_kinematic_kalman.h:

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

Go to the source code of this file.

Data Structures
struct	SimpleKinematicKalman
	Kalman structure. More...

Macros
#define	SIMPLE_KINEMATIC_KALMAN_DIM   6
#define	SIMPLE_KINEMATIC_KALMAN_SPEED_HORIZONTAL   1
#define	SIMPLE_KINEMATIC_KALMAN_SPEED_VERTICAL   2
#define	SIMPLE_KINEMATIC_KALMAN_SPEED_3D   3

Functions
void	simple_kinematic_kalman_init (struct SimpleKinematicKalman *kalman, float P0_pos, float P0_speed, float Q_sigma2, float r, float dt)
	Init SimpleKinematicKalman internal struct. More...
void	simple_kinematic_kalman_set_state (struct SimpleKinematicKalman *kalman, struct FloatVect3 pos, struct FloatVect3 speed)
	Set initial state vector. More...
void	simple_kinematic_kalman_get_state (struct SimpleKinematicKalman *kalman, struct FloatVect3 *pos, struct FloatVect3 *speed)
	Get current state. More...
struct FloatVect3	simple_kinematic_kalman_get_pos (struct SimpleKinematicKalman *kalman)
	Get current pos. More...
struct FloatVect3	simple_kinematic_kalman_get_speed (struct SimpleKinematicKalman *kalman)
	Get current speed. More...
void	simple_kinematic_kalman_update_noise (struct SimpleKinematicKalman *kalman, float Q_sigma2, float r)
	Update process and measurement noises. More...
void	simple_kinematic_kalman_predict (struct SimpleKinematicKalman *kalman)
	Prediction step. More...
void	simple_kinematic_kalman_update_pos (struct SimpleKinematicKalman *kalman, struct FloatVect3 pos)
	Update step based on each new distance data. More...
void	simple_kinematic_kalman_update_speed (struct SimpleKinematicKalman *kalman, struct FloatVect3 speed, uint8_t type)
	Update step based on speed measure. More...

Detailed Description

Author

Gautier Hattenberger gauti.nosp@m.er.h.nosp@m.atten.nosp@m.berg.nosp@m.er@en.nosp@m.ac.f.nosp@m.r

Antoine Leclerc, Nathan Puch, Pauline Molitor

Basic kinematic kalman filter for tag tracking and constant speed

Definition in file simple_kinematic_kalman.h.

---

Data Structure Documentation

SimpleKinematicKalman

struct SimpleKinematicKalman

Kalman 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 49 of file simple_kinematic_kalman.h.

Data Fields
float	dt	prediction step (in seconds)
float	F[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM]	dynamic matrix
float	Hp[SIMPLE_KINEMATIC_KALMAN_DIM/2][SIMPLE_KINEMATIC_KALMAN_DIM]	observation matrix for position
float	Hs[SIMPLE_KINEMATIC_KALMAN_DIM/2][SIMPLE_KINEMATIC_KALMAN_DIM]	observation matrix for speed
float	P[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM]	covariance matrix
float	Q[SIMPLE_KINEMATIC_KALMAN_DIM][SIMPLE_KINEMATIC_KALMAN_DIM]	process noise matrix
float	r	measurement noise (assumed the same for all anchors)
float	state[SIMPLE_KINEMATIC_KALMAN_DIM]	state vector

Macro Definition Documentation

SIMPLE_KINEMATIC_KALMAN_DIM

#define SIMPLE_KINEMATIC_KALMAN_DIM   6

Definition at line 34 of file simple_kinematic_kalman.h.

SIMPLE_KINEMATIC_KALMAN_SPEED_3D

#define SIMPLE_KINEMATIC_KALMAN_SPEED_3D   3

Definition at line 39 of file simple_kinematic_kalman.h.

SIMPLE_KINEMATIC_KALMAN_SPEED_HORIZONTAL

#define SIMPLE_KINEMATIC_KALMAN_SPEED_HORIZONTAL   1

Definition at line 37 of file simple_kinematic_kalman.h.

SIMPLE_KINEMATIC_KALMAN_SPEED_VERTICAL

#define SIMPLE_KINEMATIC_KALMAN_SPEED_VERTICAL   2

Definition at line 38 of file simple_kinematic_kalman.h.

Function Documentation

simple_kinematic_kalman_get_pos()

struct FloatVect3 simple_kinematic_kalman_get_pos

(

struct SimpleKinematicKalman *

kalman

)

Get current pos.

Parameters

[in]

kalman

SimpleKinematicKalman structure

Returns

current position

Definition at line 103 of file simple_kinematic_kalman.c.

References SimpleKinematicKalman::state, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

simple_kinematic_kalman_get_speed()

struct FloatVect3 simple_kinematic_kalman_get_speed

(

struct SimpleKinematicKalman *

kalman

)

Get current speed.

Parameters

[in]

kalman

SimpleKinematicKalman structure

Returns

current speed

Definition at line 103 of file simple_kinematic_kalman.c.

simple_kinematic_kalman_get_state()

void simple_kinematic_kalman_get_state	(	struct SimpleKinematicKalman *	kalman,
		struct FloatVect3 *	pos,
		struct FloatVect3 *	speed
	)

Get current state.

Parameters

[in]	kalman	SimpleKinematicKalman structure
[out]	pos	current position
[out]	speed	current speed

Definition at line 103 of file simple_kinematic_kalman.c.

Referenced by tag_tracking_propagate(), and update_tag_position().

Here is the caller graph for this function:

simple_kinematic_kalman_init()

void simple_kinematic_kalman_init	(	struct SimpleKinematicKalman *	kalman,
		float	P0_pos,
		float	P0_speed,
		float	Q_sigma2,
		float	r,
		float	dt
	)

Init SimpleKinematicKalman internal struct.

Parameters

[in]	kalman	SimpleKinematicKalman structure
[in]	P0_pos	initial covariance on position
[in]	P0_speed	initial covariance on speed
[in]	Q_sigma2	process noise
[in]	r	measurement noise
[in]	dt	prediction time step in seconds

Definition at line 31 of file simple_kinematic_kalman.c.

References SimpleKinematicKalman::dt, SimpleKinematicKalman::F, SimpleKinematicKalman::Hp, SimpleKinematicKalman::Hs, SimpleKinematicKalman::P, SimpleKinematicKalman::Q, SimpleKinematicKalman::r, SIMPLE_KINEMATIC_KALMAN_DIM, and SimpleKinematicKalman::state.

Referenced by tag_tracking_propagate_start_tag().

Here is the caller graph for this function:

simple_kinematic_kalman_predict()

void simple_kinematic_kalman_predict

(

struct SimpleKinematicKalman *

kalman

)

Prediction step.

Parameters

[in]

kalman

SimpleKinematicKalman 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 157 of file simple_kinematic_kalman.c.

References SimpleKinematicKalman::dt, SimpleKinematicKalman::P, SimpleKinematicKalman::Q, SIMPLE_KINEMATIC_KALMAN_DIM, and SimpleKinematicKalman::state.

Referenced by tag_tracking_propagate().

Here is the caller graph for this function:

simple_kinematic_kalman_set_state()

void simple_kinematic_kalman_set_state	(	struct SimpleKinematicKalman *	kalman,
		struct FloatVect3	pos,
		struct FloatVect3	speed
	)

Set initial state vector.

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

Parameters

[in]	kalman	SimpleKinematicKalman structure
[in]	pos	initial position
[in]	speed	initial speed

Definition at line 92 of file simple_kinematic_kalman.c.

References SimpleKinematicKalman::state, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by update_tag_position().

Here is the caller graph for this function:

simple_kinematic_kalman_update_noise()

void simple_kinematic_kalman_update_noise	(	struct SimpleKinematicKalman *	kalman,
		float	Q_sigma2,
		float	r
	)

Update process and measurement noises.

Parameters

[in]	kalman	SimpleKinematicKalman structure
[in]	Q_sigma2	process noise
[in]	r	measurement noise

Definition at line 132 of file simple_kinematic_kalman.c.

References SimpleKinematicKalman::dt, SimpleKinematicKalman::Q, SimpleKinematicKalman::r, and SIMPLE_KINEMATIC_KALMAN_DIM.

simple_kinematic_kalman_update_pos()

void simple_kinematic_kalman_update_pos	(	struct SimpleKinematicKalman *	kalman,
		struct FloatVect3	pos
	)

Update step based on each new distance data.

Parameters

[in]	kalman	SimpleKinematicKalman structure
[in]	pos	position of the target from which the distance is measured

Update step based on each new distance data.

K = P.Hpt(Hp.P.Hpt+R)^-1 = P.Hpt.S^-1 X = X + K(Z-Hp.X) P = (I-K.Hp)P

Definition at line 249 of file simple_kinematic_kalman.c.

References SimpleKinematicKalman::Hp, MAKE_MATRIX_PTR, simple_kinematic_kalman_update(), FloatVect3::x, FloatVect3::y, FloatVect3::z, and mesonh.mesonh_atmosphere::Z.

Referenced by update_tag_position().

Here is the call graph for this function:

Here is the caller graph for this function:

simple_kinematic_kalman_update_speed()

void simple_kinematic_kalman_update_speed	(	struct SimpleKinematicKalman *	kalman,
		struct FloatVect3	speed,
		uint8_t	type
	)

Update step based on speed measure.

Parameters

[in]	kalman	SimpleKinematicKalman structure
[in]	speed	new speed measurement
[in]	type	1: horizontal ground speed, 2: vertical ground speed, 3: 3D ground speed

Update step based on speed measure.

K = P.Hst(Hs.P.Hst+R)^-1 = P.Hst.S^-1 X = X + K(Z-Hs.X) P = (I-K.Hs)P

Definition at line 266 of file simple_kinematic_kalman.c.

References SimpleKinematicKalman::Hs, MAKE_MATRIX_PTR, SIMPLE_KINEMATIC_KALMAN_SPEED_HORIZONTAL, SIMPLE_KINEMATIC_KALMAN_SPEED_VERTICAL, simple_kinematic_kalman_update(), FloatVect3::x, FloatVect3::y, FloatVect3::z, and mesonh.mesonh_atmosphere::Z.

Referenced by tag_tracking_propagate().

Here is the call graph for this function:

Here is the caller graph for this function: