Kalman

Description

Kalman filter implementation.

<rc/math/kalman.h>

This may be used to implement a discrete time linear or extended kalman filter.

For the linear case, initialize the filter with rc_kalman_alloc_lin() which takes in the linear state matrices. These are then saved and used by rc_kalman_update_lin to calculate the predicted state x_pre and predicted sensor measurements h internally each step.

Basic loop structure for Linear case:

rc_kalman_t kf = rc_kalman_empty();
rc_kalman_alloc_lin(&kf,F,G,H,Q,R,Pi);
while(running){
     measure sensors, calculate y;
     rc_kalman_update_lin(&kf, u, y);
     calculate new actuator control output u;
     save u for next loop;
}
rc_kalman_free(&kf);
return;

For the non-linear EKF case, the user should initialize the filter with rc_kalman_alloc_ekf() which does NOT take in the state transition matrices F,G,H. Instead it's up to the user to calculate the new predicted state x_pre and predicted sensor measurement h for their own non-linear system model each step. Those user-calculated predictions are then given to the non-linear rc_kalman_update_ekf() function.

Basic loop structure for non-linear EKF case:

rc_kalman_t kf = rc_kalman_empty();
rc_kalman_alloc_ekf(&kf,Q,R,Pi);
while(running){
     measure sensors, calculate y
     calculate new Jacobian F given x_est from previous loop;
     calculate new predicted x_pre using x_est from previous
     loop;
     calulate new predicted sensor readings h from x_pre above;
     calculate new Jacobian H given x_pre;
     rc_kalman_update_ekf(&kf, F, x_pre, H, y, h);
     calculate new actuator control output u;
     save u for next loop;
}
rc_kalman_free(&kf);
return;

Date

April 2018

Author

Eric Nauli Sihite & James Strawson

Data Structures
struct	rc_kalman_t

Macros
#define	RC_KALMAN_INITIALIZER

Typedefs
typedef struct rc_kalman_t	rc_kalman_t

Functions
rc_kalman_t	rc_kalman_empty (void)
	Critical function for initializing rc_kalman_t structs. More...
int	rc_kalman_alloc_lin (rc_kalman_t *kf, rc_matrix_t F, rc_matrix_t G, rc_matrix_t H, rc_matrix_t Q, rc_matrix_t R, rc_matrix_t Pi)
	Allocates memory for a Kalman filter of given dimensions. More...
int	rc_kalman_alloc_ekf (rc_kalman_t *kf, rc_matrix_t Q, rc_matrix_t R, rc_matrix_t Pi)
	Allocates memory for a Kalman filter of given dimensions. More...
int	rc_kalman_free (rc_kalman_t *kf)
	Frees the memory allocated by a kalman filter's matrices and vectors. Also resets all values to 0 like rc_kalman_empty(). More...
int	rc_kalman_reset (rc_kalman_t *kf)
	reset state and dynamics of the filter to 0 More...
int	rc_kalman_update_lin (rc_kalman_t *kf, rc_vector_t u, rc_vector_t y)
	Kalman Filter state prediction step based on physical model. More...
int	rc_kalman_update_ekf (rc_kalman_t *kf, rc_matrix_t F, rc_matrix_t H, rc_vector_t x_pre, rc_vector_t y, rc_vector_t h)
	Kalman Filter measurement update step. More...

Macro Definition Documentation

RC_KALMAN_INITIALIZER

#define RC_KALMAN_INITIALIZER

Value:

{\
        .F = RC_MATRIX_INITIALIZER,\
        .G = RC_MATRIX_INITIALIZER,\
        .H = RC_MATRIX_INITIALIZER,\
        .Q = RC_MATRIX_INITIALIZER,\
        .R = RC_MATRIX_INITIALIZER,\
        .P = RC_MATRIX_INITIALIZER,\
        .Pi = RC_MATRIX_INITIALIZER,\
        .x_est = RC_VECTOR_INITIALIZER,\
        .x_pre = RC_VECTOR_INITIALIZER,\
        .initialized = 0,\
        .step = 0}

Examples:

rc_altitude.c, and rc_test_kalman.c.

Function Documentation

rc_kalman_empty()

rc_kalman_t rc_kalman_empty

(

void

)

Critical function for initializing rc_kalman_t structs.

This is a very important function. If your rc_kalman_t struct is not a global variable, then its initial contents cannot be guaranteed to be anything in particular. Therefore it could contain problematic contents which could interfere with functions in this library. Therefore, you should always initialize your filters with rc_kalman_empty() before using with any other function in this library such as rc_kalman_alloc. This serves the same purpose as rc_matrix_empty, rc_vector_empty, rc_filter_empty, and rc_ringbuf_empty.

Returns

Empty zero-filled rc_kalman_t struct

rc_kalman_alloc_lin()

int rc_kalman_alloc_lin	(	rc_kalman_t *	kf,
		rc_matrix_t	F,
		rc_matrix_t	G,
		rc_matrix_t	H,
		rc_matrix_t	Q,
		rc_matrix_t	R,
		rc_matrix_t	Pi
	)

Allocates memory for a Kalman filter of given dimensions.

Parameters

	kf	pointer to struct to be allocated
[in]	F	undriven state-transition model
[in]	G	control input model
[in]	H	observation model
[in]	Q	Process noise covariance, can be updated later
[in]	R	Measurement noise covariance, can be updated later
[in]	Pi	Initial P matrix

Returns

0 on success, -1 on failure

Examples:

rc_altitude.c, and rc_test_kalman.c.

rc_kalman_alloc_ekf()

int rc_kalman_alloc_ekf	(	rc_kalman_t *	kf,
		rc_matrix_t	Q,
		rc_matrix_t	R,
		rc_matrix_t	Pi
	)

Allocates memory for a Kalman filter of given dimensions.

Parameters

	kf	pointer to struct to be allocated
[in]	Q	Process noise covariance, can be updated later
[in]	R	Measurement noise covariance, can be updated later
[in]	Pi	Initial P matrix

Returns

0 on success, -1 on failure

rc_kalman_free()

int rc_kalman_free

(

rc_kalman_t *

kf

)

Frees the memory allocated by a kalman filter's matrices and vectors. Also resets all values to 0 like rc_kalman_empty().

Parameters

kf	pointer to user's rc_kalman_t struct

Returns

0 on success or -1 on failure.

Examples:

rc_test_kalman.c.

rc_kalman_reset()

int rc_kalman_reset

(

rc_kalman_t *

kf

)

reset state and dynamics of the filter to 0

Q and R are constant, and therefore not reset. P is set to identity*P_init.

Parameters

kf	pointer to struct to be reset

Returns

0 on success, -1 on failure

rc_kalman_update_lin()

int rc_kalman_update_lin	(	rc_kalman_t *	kf,
		rc_vector_t	u,
		rc_vector_t	y
	)

Kalman Filter state prediction step based on physical model.

Uses the state estimate and control input from the previous timestep to produce an estimate of the state at the current timestep. This step pdates P and the estimated state x. Assume that you have calculated f(x[k|k],u[k]) and F(x[k|k],u[k]) before calling this function.

• Kalman linear state prediction
  • x_pre[k|k-1] = F*x[k-1|k-1] + G*u[k-1]
  • P[k|k-1] = F*P[k-1|k-1]*F^T + Q
• Kalman measurement Update:
  • h[k] = H * x_pre[k]
  • S = H*P*H^T + R
  • L = P*(H^T)*(S^-1)
  • x_est[k|k] = x[k|k-1] + L*(y[k]-h[k])
  • P[k|k] = (I - L*H)*P[k|k-1]

Parameters

	kf	pointer to struct to be updated
	u	control input
[in]	y	sensor measurement

Returns

0 on success, -1 on failure

Examples:

rc_altitude.c, and rc_test_kalman.c.

rc_kalman_update_ekf()

int rc_kalman_update_ekf	(	rc_kalman_t *	kf,
		rc_matrix_t	F,
		rc_matrix_t	H,
		rc_vector_t	x_pre,
		rc_vector_t	y,
		rc_vector_t	h
	)

Kalman Filter measurement update step.

Updates L, P, & x_est. Assumes that you have done the non-linear prediction step in your own function which should calculate the Jacobians F(x[k|k-1]) & H(x[k|k-1]), the predicted sensor value h(x[k|k-1]), and of course the predicted state x_pre[k|k-1]

-Kalman measurement Update:

• P[k|k-1] = F*P[k-1|k-1]*F^T + Q
• S = H*P*H^T + R
• L = P*(H^T)*(S^-1)
• x[k|k] = x[k|k-1] + L*y
• P[k|k] = (I - L*H)*P

Also updates the step counter in the rc_kalman_t struct

Parameters

	kf	pointer to struct to be updated
[in]	F	Jacobian of state transition matrix linearized at x_pre
[in]	H	Jacobian of observation matrix linearized at x_pre
[in]	x_pre	predicted state
[in]	y	new sensor data
[in]	h	Ideal estimate of y, usually h=H*x_pre.

Returns

0 on success, -1 on failure