Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
linear_kalman_filter.h File Reference
#include "std.h"
+ Include dependency graph for linear_kalman_filter.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures

struct  linear_kalman_filter
 

Macros

#define KF_MAX_STATE_SIZE   6
 
#define KF_MAX_CMD_SIZE   2
 
#define KF_MAX_MEAS_SIZE   6
 

Functions

bool linear_kalman_filter_init (struct linear_kalman_filter *filter, uint8_t n, uint8_t c, uint8_t m)
 Init all matrix and vectors to zero. More...
 
void linear_kalman_filter_predict (struct linear_kalman_filter *filter, float *U)
 Prediction step. More...
 
void linear_kalman_filter_update (struct linear_kalman_filter *filter, float *Y)
 Update step. More...
 

Data Structure Documentation

◆ linear_kalman_filter

struct linear_kalman_filter

Definition at line 46 of file linear_kalman_filter.h.

Data Fields
float A[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE] dynamic matrix
float B[KF_MAX_STATE_SIZE][KF_MAX_CMD_SIZE] command matrix
float C[KF_MAX_MEAS_SIZE][KF_MAX_STATE_SIZE] observation matrix
uint8_t c command vector size (<= KF_MAX_CMD_SIZE)
uint8_t m measurement vector size (<= KF_MAX_MEAS_SIZE)
uint8_t n state vector size (<= KF_MAX_STATE_SIZE)
float P[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE] state covariance matrix
float Q[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE] proces covariance noise
float R[KF_MAX_MEAS_SIZE][KF_MAX_MEAS_SIZE] measurement covariance noise
float X[KF_MAX_STATE_SIZE] estimated state X

Macro Definition Documentation

◆ KF_MAX_CMD_SIZE

#define KF_MAX_CMD_SIZE   2

Definition at line 38 of file linear_kalman_filter.h.

◆ KF_MAX_MEAS_SIZE

#define KF_MAX_MEAS_SIZE   6

Definition at line 43 of file linear_kalman_filter.h.

◆ KF_MAX_STATE_SIZE

#define KF_MAX_STATE_SIZE   6

Definition at line 33 of file linear_kalman_filter.h.

Function Documentation

◆ linear_kalman_filter_init()

bool linear_kalman_filter_init ( struct linear_kalman_filter filter,
uint8_t  n,
uint8_t  c,
uint8_t  m 
)

Init all matrix and vectors to zero.

Parameters
filterpointer to a filter structure
nsize of the state vector
csize of the command vector
msize of the measurement vector
Returns
false if n, c or m are larger than the maximum value

Definition at line 38 of file linear_kalman_filter.c.

References linear_kalman_filter::A, linear_kalman_filter::B, linear_kalman_filter::C, linear_kalman_filter::c, float_mat_zero(), float_vect_zero(), KF_MAX_CMD_SIZE, KF_MAX_MEAS_SIZE, KF_MAX_STATE_SIZE, linear_kalman_filter::m, simple_quad_sim::m, MAKE_MATRIX_PTR, linear_kalman_filter::n, linear_kalman_filter::P, linear_kalman_filter::Q, linear_kalman_filter::R, and linear_kalman_filter::X.

Referenced by wind_estimation_quadrotor_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ linear_kalman_filter_predict()

void linear_kalman_filter_predict ( struct linear_kalman_filter filter,
float *  U 
)

Prediction step.

X = Ad * X + Bd * U P = Ad * P * Ad' + Q

Parameters
filterpointer to the filter structure
Ucommand vector

Definition at line 78 of file linear_kalman_filter.c.

References linear_kalman_filter::A, linear_kalman_filter::B, linear_kalman_filter::c, float_mat_mul(), float_mat_mul_transpose(), float_mat_sum(), float_mat_vect_mul(), float_vect_sum(), MAKE_MATRIX_PTR, linear_kalman_filter::n, linear_kalman_filter::P, linear_kalman_filter::Q, and linear_kalman_filter::X.

Referenced by wind_estimation_quadrotor_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ linear_kalman_filter_update()

void linear_kalman_filter_update ( struct linear_kalman_filter filter,
float *  Y 
)

Update step.

S = Cd * P * Cd' + R K = P * Cd' / S X = X + K * (Y - Cd * X) P = P - K * Cd * P

Parameters
filterpointer to the filter structure
Ymeasurement vector

Definition at line 111 of file linear_kalman_filter.c.

References linear_kalman_filter::C, float_mat_diff(), float_mat_invert(), float_mat_mul(), float_mat_mul_copy(), float_mat_mul_transpose(), float_mat_sum(), float_mat_vect_mul(), float_vect_diff(), float_vect_sum(), K, linear_kalman_filter::m, MAKE_MATRIX_PTR, linear_kalman_filter::n, linear_kalman_filter::P, linear_kalman_filter::R, linear_kalman_filter::X, and mesonh.mesonh_atmosphere::Y.

Referenced by wind_estimation_quadrotor_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function: