Location via proxy:   [ UP ]  
[Report a bug]   [Manage cookies]                
Scribd Logo
Upload

Welcome to Scribd!

  • 99 views

    Kalman C

    Uploaded by

    cartamenes
    This document contains code for implementing three types of Kalman filters: a linear Kalman filter, an extended Kalman filter, and an iterated extended Kalman filter. It includes functions for initializing the filters, taking measurement steps, and accessing the state estimates. The filters use matrix and vector operations to estimate state probabilities and update the state based on measurements.

    Copyright:

    © All Rights Reserved
    Download as TXT, PDF, TXT or read online from Scribd

    Kalman C

    Uploaded by

    cartamenes
    99 views7 pages
    This document contains code for implementing three types of Kalman filters: a linear Kalman filter, an extended Kalman filter, and an iterated extended Kalman filter. It includes functions for initializing the filters, taking measurement steps, and accessing the state estimates. The filters use matrix and vector operations to estimate state probabilities and update the state based on measurements.

    Original Description:

    kalman filter code

    Original Title

    kalman.c

    Copyright

    © © All Rights Reserved

    Available Formats

    TXT, PDF, TXT or read online from Scribd

    Did you find this document useful?

    Is this content inappropriate?

    This document contains code for implementing three types of Kalman filters: a linear Kalman filter, an extended Kalman filter, and an iterated extended Kalman filter. It includes functions for initializing the filters, taking measurement steps, and accessing the state estimates. The filters use matrix and vector operations to estimate state probabilities and update the state based on measurements.

    Copyright:

    © All Rights Reserved
    Download as TXT, PDF, TXT or read online from Scribd
    Download as txt, pdf, or txt
    99 views7 pages

    Kalman C

    Uploaded by

    cartamenes
    This document contains code for implementing three types of Kalman filters: a linear Kalman filter, an extended Kalman filter, and an iterated extended Kalman filter. It includes functions for initializing the filters, taking measurement steps, and accessing the state estimates. The filters use matrix and vector operations to estimate state probabilities and update the state based on measurements.

    Copyright:

    © All Rights Reserved
    Download as TXT, PDF, TXT or read online from Scribd
    Download as txt, pdf, or txt
    You are on page 1of 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

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

    You might also like