kalman_UKF.cpp

Go to the documentation of this file.

 
#include "sigpack.h"
 
using namespace std;
using namespace sp;
 
int main()
{
    // Number of samples
    arma::uword Nsamp = 120;
    arma::uword N     = 6; // Nr of states [X,Y,Vx,Vy,Ax,Ay]
    arma::uword M     = 2; // Nr of measurements
    arma::uword L     = 0; // Nr of inputs
 
    //    // Instatiate a Extended Kalman Filter
    //    EKF kalman(N,M,L);
    // Instatiate an Unscented Kalman Filter
    UKF kalman( N, M, L );
 
    // Initialisation and setup of system
    double P0 = 50;
    double Q0 = 0.001;
    double R0 = 8;
 
    // Meas interval
    double dT = 0.1;
 
    // [X,Y,Vx,Vy,Ax,Ay] 2D position, velocity and acceleration as states
    arma::mat x = { 0, 5, 10, 50, -0.5, -9.8 };
    arma::inplace_trans( x );
    arma::mat x_init( x * 0.5 ); // Init state as 50% of actual value
    kalman.set_state_vec( x_init );
 
    // Set transition function
    arma::mat A = { { 1, 0, dT, 0, dT * dT / 2, 0 }, { 0, 1, 0, dT, 0, dT * dT / 2 }, { 0, 0, 1, 0, dT, 0 }, { 0, 0, 0, 1, 0, dT }, { 0, 0, 0, 0, 1, 0 }, { 0, 0, 0, 0, 0, 1 } };
    kalman.set_trans_mat( A );
    //    fcn_t f0 = FCN_XUW{ return x(0)+x(2)*dT+x(4)*dT*dT/2; };
    //    fcn_t f1 = FCN_XUW{ return x(1)+x(3)*dT+x(5)*dT*dT/2; };
    //    fcn_t f2 = FCN_XUW{ return x(2)+x(4)*dT; };
    //    fcn_t f3 = FCN_XUW{ return x(3)+x(5)*dT; };
    //    fcn_t f4 = FCN_XUW{ return x(4); };
    //    fcn_t f5 = FCN_XUW{ return x(5); };
    //    fcn_v  f = {f0,f1,f2,f3,f4,f5};
    //    kalman.set_trans_fcn(f);
 
    // Set process noise
    arma::mat Q = Q0 * arma::diagmat( arma::vec( "1 1 0.1 0.1 0.01 0.01" ) );
    kalman.set_proc_noise( Q );
 
    // Nonlinear measurement function
    fcn_t h0 = FCN_XUW
    {
        return sqrt( x( 0 ) * x( 0 ) + x( 1 ) * x( 1 ) );
    };
    fcn_t h1 = FCN_XUW
    {
        return atan2( x( 1 ), x( 0 ) );
    };
    fcn_v h = { h0, h1 };
    kalman.set_meas_fcn( h );
 
    // Nonlinear measurement jacobian - analytical
    //  fcn_t h_zero   = FCN_XUW{ return 0; };
    //  fcn_t h00_j    = FCN_XUW{ return x(0)/sqrt(x(0)*x(0)+x(1)*x(1)); };
    //  fcn_t h01_j    = FCN_XUW{ return x(1)/sqrt(x(0)*x(0)+x(1)*x(1)); };
    //  fcn_t h10_j    = FCN_XUW{ return -x(1)/(x(0)*x(0)+x(1)*x(1)); };
    //  fcn_t h11_j    = FCN_XUW{ return x(0)/(x(0)*x(0)+x(1)*x(1)); };
    //  fcn_m h_jac =
    //  {
    //        {h00_j,h01_j,h_zero,h_zero,h_zero,h_zero},
    //        {h10_j,h11_j,h_zero,h_zero,h_zero,h_zero}
    //  };
    //  kalman.set_meas_jac(h_jac);
 
    // Error cov
    arma::mat P = P0 * arma::eye<arma::mat>( N, N );
    kalman.set_err_cov( P );
 
    // Meas noise  (for distance and angle)
    arma::mat R = { { 1, 0 }, { 0, 0.005 } };
    R *= R0;
    kalman.set_meas_noise( R );
 
    // Create simulation data
    arma::mat z( M, Nsamp, arma::fill::zeros );
    arma::mat z0( M, Nsamp, arma::fill::zeros );
    for( arma::uword n = 0; n < Nsamp; n++ )
    {
        arma::mat v0( N, 1, arma::fill::zeros );
 
        // Update state
        x = A * x + Q * arma::randn( N, 1 );
 
        // Update measurement
        z0( 0, n ) = x( 0 );
        z0( 1, n ) = x( 1 );
 
        // Add meas noise
        z.col( n ) = eval_fcn( h, z0.col( n ) ) + 0.5 * R * arma::randn( M, 1 );
    }
 
    arma::mat  xhat_log( N, Nsamp );
    arma::mat  e_log( M, Nsamp );
    arma::cube P_log( N, N, Nsamp );
    arma::mat  xs_log( M, Nsamp );
    arma::cube Ps_log( N, N, Nsamp );
    arma::cube K_log( N, M, Nsamp );
 
    // Kalman filter loop
    for( arma::uword n = 0; n < Nsamp; n++ )
    {
        // Update
        kalman.update( z.col( n ) );
        xhat_log.col( n ) = kalman.get_state_vec();
 
        // Predict
        kalman.predict();
 
        e_log.col( n )   = kalman.get_err();
        P_log.slice( n ) = kalman.get_err_cov();
        K_log.slice( n ) = kalman.get_kalman_gain();
    }
 
    // RTS smoother
    kalman.rts_smooth( xhat_log, P_log, xs_log, Ps_log );
 
    // Display result
    gplot gp0;
    gp0.window( "Plot", 10, 10, 500, 500 );
    gp0.set_term( "qt" );
    gp0.plot_add( z0.row( 0 ), z0.row( 1 ), "True Y", "lines dashtype 2 linecolor \"black\"" );
    gp0.plot_add( arma::mat( z.row( 0 ) % cos( z.row( 1 ) ) ), arma::mat( z.row( 0 ) % sin( z.row( 1 ) ) ), "Meas Y", "points" );
    gp0.plot_add( xhat_log.row( 0 ), xhat_log.row( 1 ), "Kalman x hat" );
    gp0.plot_add( xs_log.row( 0 ), xs_log.row( 1 ), "RTS smooth" );
    gp0.plot_show();
 
    gplot gp1;
    gp1.window( "Plot", 800, 10, 500, 500 );
    gp1.set_term( "qt" );
    gp1.plot_add( xhat_log.row( 4 ), "Acc x" );
    gp1.plot_add( xhat_log.row( 5 ), "Acc y" );
    gp1.plot_show();
 
    gplot gp2;
    gp2.window( "Plot", 800, 10, 500, 500 );
    gp2.set_term( "qt" );
 
    arma::mat ppp = P_log.tube( 0, 0 );
    gp2.plot_add( ppp, "P  x" );
    ppp = P_log.tube( 1, 1 );
    gp2.plot_add( ppp, "P  y" );
    ppp = P_log.tube( 2, 2 );
    gp2.plot_add( ppp, "P Vx" );
    ppp = P_log.tube( 3, 3 );
    gp2.plot_add( ppp, "P Vy" );
    ppp = P_log.tube( 4, 4 );
    gp2.plot_add( ppp, "P Ax" );
    ppp = P_log.tube( 5, 5 );
    gp2.plot_add( ppp, "P Ay" );
    gp2.plot_show();
 
    return 0;
}