Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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]kalmanSimpleKinematicKalman structure
Returns
current position

Definition at line 103 of file simple_kinematic_kalman.c.

References kalman, 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]kalmanSimpleKinematicKalman 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]kalmanSimpleKinematicKalman structure
[out]poscurrent position
[out]speedcurrent 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]kalmanSimpleKinematicKalman structure
[in]P0_posinitial covariance on position
[in]P0_speedinitial covariance on speed
[in]Q_sigma2process noise
[in]rmeasurement noise
[in]dtprediction time step in seconds

Definition at line 31 of file simple_kinematic_kalman.c.

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

Referenced by tag_tracking_propagate_start().

+ Here is the caller graph for this function:

◆ simple_kinematic_kalman_predict()

void simple_kinematic_kalman_predict ( struct SimpleKinematicKalman kalman)

Prediction step.

Parameters
[in]kalmanSimpleKinematicKalman 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, kalman, 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]kalmanSimpleKinematicKalman structure
[in]posinitial position
[in]speedinitial speed

Definition at line 92 of file simple_kinematic_kalman.c.

References kalman, 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]kalmanSimpleKinematicKalman structure
[in]Q_sigma2process noise
[in]rmeasurement noise

Definition at line 132 of file simple_kinematic_kalman.c.

References SimpleKinematicKalman::dt, kalman, 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]kalmanSimpleKinematicKalman structure
[in]posposition 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, kalman, 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]kalmanSimpleKinematicKalman structure
[in]speednew speed measurement
[in]type1: 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, kalman, 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: