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

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: