/* 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

<stdio.h>
<stdlib.h>
<math.h>
"kalman.h"

#define ITERATION_THRESHOLD
#define ITERATION_DIVERGENCE

2.0
20

/* The following are the global variables of the Kalman filters,

used to point to data structures used throughout.
*/
static m_elem *state_pre;
static m_elem *state_post;

/* ptr to apriori state vectors, x(-)

*/
/* ptr to aposteriori state vectors, x(+) */

static m_elem *iter_state0;

static m_elem *iter_state1;
static
static
static
static

m_elem
m_elem
m_elem
m_elem

**cov_pre;
**cov_post;
**sys_noise_cov;
**mea_noise_cov;

/*
/*
/*
/*

ptr to apriori covariance matrix, P(-)

ptr to apriori covariance matrix, P(-)
system noise covariance matrix (GQGt)
measurement noise variance vector (R)

static m_elem **sys_transfer;

static m_elem **mea_transfer;

/* system transfer function (Phi)

*/
/* measurement transfer function (H) */

static m_elem **kalman_gain;

/* The Kalman Gain matrix (K) */

int
int
int

/* the current step number (k) */

/* number of elems in measurement */
/* number of elements in state
*/

global_step = 0;
measurement_size;
state_size;

*/
*/
*/
*/

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

memory allocation.
*/
static
static
static
static
static

m_elem
m_elem
m_elem
m_elem
m_elem

*z_estimate;
**temp_state_state;
**temp_meas_state;
**temp_meas_meas;
**temp_meas_2;

/*
/*
/*
/*
/*

a measurement_size x 1 vector */
a state_size x state_size matrix */
a measurement_size x state_size matrix */
a measurement_size squared matrix */
another one ! */

/* Prototypes of internal functions */

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++;
}

/* 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 );
/* Init the global variables using the arguments. */
vec_copy(
vec_copy(
mat_copy(
mat_copy(

x,
x,
P,
P,

state_post, state_size );
state_pre, state_size );
cov_post, state_size, state_size );
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 );

/************** 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
iter_state1
sys_transfer
mea_transfer

=
=
=
=

vector(
vector(
matrix(
matrix(

1,
1,
1,
1,

num_state );
num_state );
num_state, 1, num_state );
num_measurement, 1, num_state );

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

vec_copy(
vec_copy(
mat_copy(
mat_copy(

x,
x,
P,
P,

state_post, state_size );
state_pre, state_size );
cov_post, state_size, state_size );
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;

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

*/

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

#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 <= state_size; m++ )
{
acc += fabs( a[m] - b[m] );
}
return( acc );
}