- /* 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 <stdio.h>
- #include <stdlib.h>
- #include <math.h>
- #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 */
- 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( 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 );
- /************** 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 );
- /* Init the global variables using the arguments. */
- 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;
- 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) &&
全部代码51hei下载地址：
C语言卡尔曼滤波.zip
(11.45 KB, 下载次数: 58)
|
[复制链接]
