kalman.c

8/17/2019 kalman.c

1/7

/* kalman.c

This file contains the code for a kalman filter, an extended kalman filter, and an iterated extended kalman filter.

For ready extensibility, the apply_measurement() and apply_system() functions are located in a separate file: kalman_cam.c is an example.

It uses the matmath functions provided in an accompanying file to perform matrix and quaternion manipulation.

J. Watlington, 11/15/95

Modified: 11/30/95 wad The extended kalman filter section seems to be working now.*/

#include #include #include #include "kalman.h"

#define ITERATION_THRESHOLD 2.0#define ITERATION_DIVERGENCE 20

/* The following are the global variables of the Kalman filters, used to point to data structures used throughout. */

static m_elem *state_pre; /* ptr to apriori state vectors, x(-) */static m_elem *state_post; /* ptr to aposteriori state vectors, x(+) */

static m_elem *iter_state0;static m_elem *iter_state1;

static m_elem **cov_pre; /* ptr to apriori covariance matrix, P(-) */

static m_elem **cov_post; /* ptr to apriori covariance matrix, P(-) */static m_elem **sys_noise_cov; /* system noise covariance matrix (GQGt) */static m_elem **mea_noise_cov; /* measurement noise variance vector (R) */

static m_elem **sys_transfer; /* system transfer function (Phi) */static m_elem **mea_transfer; /* measurement transfer function (H) */

static m_elem **kalman_gain; /* The Kalman Gain matrix (K) */

int global_step = 0; /* the current step number (k) */int measurement_size; /* number of elems in measurement */int state_size; /* number of elements in state */

/* Temporary variables, declared statically to avoid lots of run-time memory allocation. */

static m_elem *z_estimate; /* a measurement_size x 1 vector */static m_elem **temp_state_state; /* a state_size x state_size matrix */static m_elem **temp_meas_state; /* a measurement_size x state_size matrix */static m_elem **temp_meas_meas; /* a measurement_size squared matrix */static m_elem **temp_meas_2; /* another one ! */

/* Prototypes of internal functions */

8/17/2019 kalman.c

2/7

static void alloc_globals( int num_state, int num_measurement );

static void update_system( m_elem *z, m_elem *x_minus, m_elem **kalman_gain, m_elem *x_plus );

static void estimate_prob( m_elem **P_post, m_elem **Phi, m_elem **GQGt, m_elem **P_pre );

static void update_prob( m_elem **P_pre, m_elem **R, m_elem **H,m_elem **P_post, m_elem **K );

static void take_inverse( m_elem **in, m_elem **out, int n );static m_elem calc_state_change( m_elem *a, m_elem *b );

/******************************************************************

Linear Kalman Filtering

kalman_init() This function initializes the kalman filter. Note that for a straight-forward (linear) Kalman filter, this is the only place that K and P are computed... */

void kalman_init( m_elem **GQGt, m_elem **Phi, m_elem **H, m_elem **R,m_elem **P, m_elem *x, int num_state, int num_measurement )

{ alloc_globals( num_state, num_measurement );

/* Init the global variables using the arguments. */

vec_copy( x, state_post, state_size ); mat_copy( P, cov_post, state_size, state_size );

sys_noise_cov = GQGt; mea_noise_cov = R;

sys_transfer = Phi; mea_transfer = H;

/***************** Gain Loop *****************/

estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre ); update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain );}

/* kalman_step() This function takes a set of measurements, and performs a single recursion of the straight-forward kalman filter.*/

void kalman_step( m_elem *z_in ){ /************** Estimation Loop ***************/

apply_system( state_post, state_pre ); update_system( z_in, state_pre, kalman_gain, state_post );

global_step++;}

8/17/2019 kalman.c

3/7

/* kalman_get_state This function returns a pointer to the current estimate (a posteriori) of the system state. */

m_elem *kalman_get_state( void ){ return( state_post );}

/******************************************************************

Non-linear Kalman Filtering

extended_kalman_init() This function initializes the extended kalman filter.*/

void extended_kalman_init( m_elem **GQGt, m_elem **R, m_elem **P, m_elem *x, int num_state, int num_measurement )

{#ifdef PRINT_DEBUG printf( "ekf: Initializing filter\n" );#endif

alloc_globals( num_state, num_measurement );

sys_transfer = matrix( 1, num_state, 1, num_state ); mea_transfer = matrix( 1, num_measurement, 1, num_state );


vec_copy( x, state_post, state_size ); vec_copy( x, state_pre, state_size ); mat_copy( P, cov_post, state_size, state_size ); mat_copy( P, cov_pre, state_size, state_size );

sys_noise_cov = GQGt;

mea_noise_cov = R;}

/* extended_kalman_step() This function takes a set of measurements, and performs a single recursion of the extended kalman filter.*/

void extended_kalman_step( m_elem *z_in ){#ifdef PRINT_DEBUG printf( "ekf: step %d\n", global_step );

#endif /***************** Gain Loop ***************** First, linearize locally, then do normal gain loop */

generate_system_transfer( state_pre, sys_transfer ); generate_measurement_transfer( state_pre, mea_transfer );

estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre ); update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain );

8/17/2019 kalman.c

4/7

/************** Estimation Loop ***************/

apply_system( state_post, state_pre ); update_system( z_in, state_pre, kalman_gain, state_post );

global_step++;}

/* iter_ext_kalman_init() This function initializes the iterated extended kalman filter*/

void iter_ext_kalman_init( m_elem **GQGt, m_elem **R, m_elem **P, m_elem *x, int num_state, int num_measurement )

{#ifdef PRINT_DEBUG printf( "iekf: Initializing filter\n" );#endif

alloc_globals( num_state, num_measurement );

iter_state0 = vector( 1, num_state ); iter_state1 = vector( 1, num_state );

sys_transfer = matrix( 1, num_state, 1, num_state ); mea_transfer = matrix( 1, num_measurement, 1, num_state );


vec_copy( x, state_post, state_size ); vec_copy( x, state_pre, state_size ); mat_copy( P, cov_post, state_size, state_size ); mat_copy( P, cov_pre, state_size, state_size );

sys_noise_cov = GQGt; mea_noise_cov = R;}

/* iter_ext_kalman_step() This function takes a set of measurements, and iterates over a single recursion of the extended kalman filter.*/

void iter_ext_kalman_step( m_elem *z_in ){ int iteration = 1; m_elem est_change; m_elem *prev_state; m_elem *new_state; m_elem *temp;

generate_system_transfer( state_pre, sys_transfer ); estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre ); apply_system( state_post, state_pre );

/* Now iterate, updating the probability and the system model until no change is noticed between iteration steps */

prev_state = iter_state0; new_state = iter_state1;

8/17/2019 kalman.c

5/7

generate_measurement_transfer( state_pre, mea_transfer ); update_prob( cov_pre, mea_noise_cov, mea_transfer,

cov_post, kalman_gain ); update_system( z_in, state_pre, kalman_gain, prev_state ); est_change = calc_state_change( state_pre, prev_state );

while( (est_change < ITERATION_THRESHOLD) &&(iteration++ < ITERATION_DIVERGENCE) )

{#ifdef DEBUG_ITER

print_vector( "\titer state", prev_state, 10 );#endif /* Update the estimate */

generate_measurement_transfer( prev_state, mea_transfer ); update_prob( cov_pre, mea_noise_cov, mea_transfer,

cov_post, kalman_gain ); update_system( z_in, prev_state, kalman_gain, new_state ); est_change = calc_state_change( prev_state, new_state );

temp = prev_state; prev_state = new_state; new_state = temp;

}

vec_copy( prev_state, state_post, state_size );

#ifdef PRINT_DEBUG printf( "iekf: step %3d, %2d iters\n", global_step, iteration );#endif global_step++;}

/************************************************************

Internal Functions, defined in order of appearance

alloc_globals() This function allocates memory for the global variables used by this code module. */

static void alloc_globals( int num_state, int num_measurement ){#ifdef PRINT_DEBUG printf( "ekf: allocating memory\n" );#endif state_size = num_state; measurement_size = num_measurement;

/* Allocate some global variables. */

state_pre = vector( 1, state_size ); state_post = vector( 1, state_size ); cov_pre = matrix( 1, state_size, 1, state_size ); cov_post = matrix( 1, state_size, 1, state_size ); kalman_gain = matrix( 1, state_size, 1, measurement_size );

/* Alloc some temporary variables */

8/17/2019 kalman.c

6/7

z_estimate = vector( 1, measurement_size ); temp_state_state = matrix( 1, state_size, 1, state_size ); temp_meas_state = matrix( 1, measurement_size, 1, state_size ); temp_meas_meas = matrix( 1, measurement_size, 1, measurement_size ); temp_meas_2 = matrix( 1, measurement_size, 1, measurement_size );} /* update_system() This function generates an updated version of the state estimate, based on what we know about the measurement system. */

static void update_system( m_elem *z, m_elem *x_pre, m_elem **K, m_elem *x_post )

{#ifdef PRINT_DEBUG printf( "ekf: updating system\n" );#endif

apply_measurement( x_pre, z_estimate ); vec_sub( z, z_estimate, z_estimate, measurement_size ); mat_mult_vector( K, z_estimate, x_post, state_size, measurement_size ); vec_add( x_post, x_pre, x_post, state_size );}

/* estimate_prob() This function estimates the change in the variance of the state variables, given the system transfer function. */

static void estimate_prob( m_elem **P_post, m_elem **Phi, m_elem **GQGt, m_elem **P_pre )

{#ifdef PRINT_DEBUG printf( "ekf: estimating prob\n" );#endif

mat_mult_transpose( P_post, Phi, temp_state_state, state_size, state_size, state_size );

mat_mult( Phi, temp_state_state, P_pre, state_size, state_size, state_size ); mat_add( P_pre, GQGt, P_pre, state_size, state_size );}

/* update_prob() This function updates the state variable variances. Inputs: P_pre - the apriori probability matrix ( state x state ) R - measurement noise covariance ( meas x meas ) H - the measurement transfer matrix ( meas x state ) Outputs:

P_post - the aposteriori probability matrix (state x state ) K - the Kalman gain matrix ( state x meas )*/

static void update_prob( m_elem **P_pre, m_elem **R, m_elem **H,m_elem **P_post, m_elem **K )

{#ifdef PRINT_DEBUG printf( "ekf: updating prob\n" );#endif

8/17/2019 kalman.c

7/7

#ifdef DIV_DEBUG print_matrix( "P", P_pre, state_size, state_size );#endif mat_mult( H, P_pre, temp_meas_state,

measurement_size, state_size, state_size ); mat_mult_transpose( H, temp_meas_state, temp_meas_meas,

measurement_size, state_size, measurement_size ); mat_add( temp_meas_meas, R, temp_meas_meas,

measurement_size, measurement_size );

take_inverse( temp_meas_meas, temp_meas_2, measurement_size );

#ifdef DIV_DEBUG print_matrix( "1 / (HPH + R)", temp_meas_2,

measurement_size, measurement_size );#endif mat_transpose_mult( temp_meas_state, temp_meas_2, K,

state_size, measurement_size, measurement_size );

/* print_matrix( "Kalman Gain", K, state_size, measurement_size );*/ mat_mult( K, temp_meas_state, temp_state_state,

state_size, measurement_size, state_size );

#ifdef PRINT_DEBUG printf( "ekf: updating prob 3\n" );#endif mat_add( temp_state_state, P_pre, P_post, state_size, state_size );}

static void take_inverse( m_elem **in, m_elem **out, int n ){#ifdef PRINT_DEBUG printf( "ekf: calculating inverse\n" );#endif /* Nothing fancy for now, just a Gauss-Jordan technique,

with good pivoting (thanks to NR). */

gaussj( in, n, out, 0 ); /* out is SCRATCH */ mat_copy( in, out, n, n );}

static m_elem calc_state_change( m_elem *a, m_elem *b ){ int m; m_elem acc = 0.0;

for( m = 1; m

Documents

kalman.c