simple_kinematic_kalman.c File Reference

#include "filters/simple_kinematic_kalman.h"
#include <math.h>

Include dependency graph for simple_kinematic_kalman.c:

Go to the source code of this file.

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.
void	simple_kinematic_kalman_set_state (struct SimpleKinematicKalman *kalman, struct FloatVect3 pos, struct FloatVect3 speed)
	Set initial state vector.
void	simple_kinematic_kalman_get_state (struct SimpleKinematicKalman *kalman, struct FloatVect3 *pos, struct FloatVect3 *speed)
	Get current state.
struct FloatVect3	simple_kinematic_kalman_get_pos (struct SimpleKinematicKalman *kalman)
	Get current pos.
struct FloatVect3	simple_kinematic_kalman_get_speed (struct SimpleKinematicKalman *kalman)
	Get current speed.
void	simple_kinematic_kalman_update_noise (struct SimpleKinematicKalman *kalman, float Q_sigma2, float r)
	Update process and measurement noises.
void	simple_kinematic_kalman_predict (struct SimpleKinematicKalman *kalman)
	propagate dynamic model
static void	simple_kinematic_kalman_update (struct SimpleKinematicKalman *kalman, float **_H, float *Z)
	generic correction step
void	simple_kinematic_kalman_update_pos (struct SimpleKinematicKalman *kalman, struct FloatVect3 pos)
	correction step for position
void	simple_kinematic_kalman_update_speed (struct SimpleKinematicKalman *kalman, struct FloatVect3 speed, uint8_t type)
	correction step for speed

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.c.

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

References 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 123 of file simple_kinematic_kalman.c.

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

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.

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

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, foo, 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

)

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

References SimpleKinematicKalman::dt, foo, 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()

static void simple_kinematic_kalman_update ( struct SimpleKinematicKalman *  kalman, float **  _H, float *  Z  )

static

generic correction step

K = PHt(HPHt+R)^-1 = PHtS^-1 X = X + K(Z-HX) P = (I-KH)P

Parameters

	kalman	pointer to kalman structure
[in]	H	pointer to observation matrix
[in]	Z	pointer to measurement vector

Definition at line 186 of file simple_kinematic_kalman.c.

References float_mat_invert(), float_mat_mul(), float_mat_transpose(), float_mat_vect_mul(), float_vect_add(), float_vect_diff(), foo, K, MAKE_MATRIX_PTR, SimpleKinematicKalman::P, SimpleKinematicKalman::r, and SimpleKinematicKalman::state.

Referenced by simple_kinematic_kalman_update_pos(), and simple_kinematic_kalman_update_speed().

Here is the call graph for this function:

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, foo, 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
	)

correction step for position

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 foo, SimpleKinematicKalman::Hp, MAKE_MATRIX_PTR, simple_kinematic_kalman_update(), FloatVect3::x, FloatVect3::y, and FloatVect3::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
	)

correction step for 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 foo, SimpleKinematicKalman::Hs, MAKE_MATRIX_PTR, SIMPLE_KINEMATIC_KALMAN_SPEED_HORIZONTAL, SIMPLE_KINEMATIC_KALMAN_SPEED_VERTICAL, simple_kinematic_kalman_update(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by tag_tracking_propagate().

Here is the call graph for this function:

Here is the caller graph for this function: