linear_kalman_filter.c File Reference

Generic discrete Linear Kalman Filter. More...

#include "filters/linear_kalman_filter.h"
#include "math/pprz_algebra_float.h"

Include dependency graph for linear_kalman_filter.c:

Go to the source code of this file.

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

Detailed Description

Generic discrete Linear Kalman Filter.

Definition in file linear_kalman_filter.c.

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

filter	pointer to a filter structure
n	size of the state vector
c	size of the command vector
m	size 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

filter	pointer to the filter structure
U	command 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

filter	pointer to the filter structure
Y	measurement 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: