kalman_main.cc

Go to the documentation of this file.

/* MLPACK 0.2
 *
 * Copyright (c) 2008, 2009 Alexander Gray,
 *                          Garry Boyer,
 *                          Ryan Riegel,
 *                          Nikolaos Vasiloglou,
 *                          Dongryeol Lee,
 *                          Chip Mappus, 
 *                          Nishant Mehta,
 *                          Hua Ouyang,
 *                          Parikshit Ram,
 *                          Long Tran,
 *                          Wee Chin Wong
 *
 * Copyright (c) 2008, 2009 Georgia Institute of Technology
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301, USA.
 */
#include <iostream>
#include "fastlib/fastlib.h"
#include "kalman_helper.h"
#include "kalman.h"
using namespace std;

int main(int argc, char* argv[]) {

  fx_init(argc, argv, NULL);
 

  // User-specified: duration of experiment 
  // Note that the experiment will run from t=0 to t_tot
  const char* t_in = fx_param_str(NULL, "t_in", "t_in");
  Matrix t_tot_mat; data::Load(t_in, &t_tot_mat);
  int t_tot = (int)t_tot_mat.get(0, 0);
  
  // User-specified: system parameters. Stored in *.csv files 
  // a_mat, b_mat, c_mat, d_mat, q_mat, r_mat, s_mat
  // are the typical time-invariant system params
  // Proceed to Load init matrices within a struct called lds
  const char* a_in = fx_param_str(NULL, "a_in", "a_in");
  const char* b_in = fx_param_str(NULL, "b_in", "b_in");
  const char* c_in = fx_param_str(NULL, "c_in", "c_in");
  const char* q_in = fx_param_str(NULL, "q_in", "q_in");
  const char* r_in = fx_param_str(NULL, "r_in", "r_in");
  const char* s_in = fx_param_str(NULL, "s_in", "s_in");  
  
  ssm lds;
  data::Load(a_in, &lds.a_mat); 
  data::Load(b_in, &lds.b_mat); 
  data::Load(c_in, &lds.c_mat); 
  data::Load(q_in, &lds.q_mat); 
  data::Load(r_in, &lds.r_mat); 
  data::Load(s_in, &lds.s_mat);  
             
  // User-specified: kf parameters. Stored in *.csv files 
  // Includes x_{0|-1}, p_pred_{0|-1}, y_{0|-1}, inno_cov_{0|-1}
  // {t|t-1} means var. at time t given info. up to t-1
  const char* x_pred_0_in = fx_param_str(NULL, "x_pred_0_in", "x_pred_0_in");
  const char* p_pred_0_in = fx_param_str(NULL, "p_pred_0_in", "p_pred_0_in");
  const char* y_pred_0_in = fx_param_str(NULL, "y_pred_0_in", "y_pred_0_in");
  const char* inno_cov_0_in = fx_param_str(NULL, "inno_cov_0_in", 
                                           "inno_cov_0_in");
  
  Matrix x_pred_0; data::Load(x_pred_0_in, &x_pred_0);
  Matrix y_pred_0; data::Load(y_pred_0_in, &y_pred_0); 
  Matrix p_pred_0; data::Load(p_pred_0_in, &p_pred_0);
  Matrix inno_cov_0; data::Load(inno_cov_0_in, &inno_cov_0);

  // Set up parameters to be used by signal generator. 
  // Assumed to be an lds with the same params. as the KF
  // for the purpose of demonstration
  // assume that x is initially zero
  int nx = lds.a_mat.n_rows(); 
  int ny = lds.c_mat.n_rows(); 
  int nu = lds.b_mat.n_cols();
  Matrix x(nx, t_tot + 1); x.SetZero();
  Matrix y(ny, t_tot + 1); 
  Matrix u(nu, t_tot + 1); 
  Matrix w(nx, t_tot + 1); 
  Matrix v(ny, t_tot + 1); 

  // Set up params. for the KF including:
  // estimates: x_{t|t-1}, x_{t|t}, y_{t|t-1}
  // error covariances: p_pred_{t|t-1}, p_hat{t|t-1}, inno_cov_{t|t-1}
  // Kalman gain
  // Also initialize values and sizes where appropriate
  Matrix x_pred(nx, t_tot + 1); 
  Matrix x_hat(nx, t_tot + 1);
  Matrix y_pred(ny, t_tot + 1);
  Matrix p_pred[t_tot + 1]; 
  Matrix p_hat[t_tot + 1];
  Matrix inno_cov[t_tot + 1];
  Matrix k_gain[t_tot + 1];
  set_portion_of_matrix(x_pred_0, 0, nx-1, 0, 0, &x_pred);
  set_portion_of_matrix(y_pred_0, 0, ny-1, 0, 0, &y_pred);
  for (int t =0; t<=t_tot; t++ ) {
    p_pred[t].Init(nx, nx);
    p_hat[t].Init(nx, nx);
    k_gain[t].Init(nx, ny);
    inno_cov[t].Init(ny, ny);
  };    
  p_pred[0]   = p_pred_0;
  inno_cov[0] = inno_cov_0;

  // Define other params. to be used
  // noise_matrix = [q_mat s_mat; s_mat' r_mat]
  Matrix noise_mat(nx + ny, nx + ny), s_trans;
  la:: TransposeInit(lds.s_mat, &s_trans);
  set_portion_of_matrix(lds.q_mat, 0, nx - 1, 0, nx - 1, &noise_mat);
  set_portion_of_matrix(lds.s_mat, 0, nx - 1, nx, nx + ny -1, &noise_mat);
  set_portion_of_matrix(s_trans, nx, nx + ny - 1,0, nx - 1, &noise_mat);
  set_portion_of_matrix(lds.r_mat, nx, nx + ny - 1, nx, nx + ny - 1, &noise_mat); 


  for (int t=0; t<=t_tot; t++) {
    /*
      Signal generation. As a demo, the system will also 
      be an lds with the same parameters as the k.f. 
      although this doesn't need to be necessarily true */
    
    // Generate noise
    // Also make aliases which do not need to be init'd
    Vector w_t; w.MakeColumnVector(t, &w_t); 
    Vector v_t; v.MakeColumnVector(t, &v_t);    
    Vector w_v_t; w_v_t.Init(nx+ny);
    RandVector(noise_mat, w_v_t);
    extract_sub_vector_of_vector(w_v_t, 0, nx-1, &w_t);
    extract_sub_vector_of_vector(w_v_t, nx, nx+ny-1, &v_t);  

    // y_t = c_mat*x_{t} +  v_t 
    // Also make aliases which do not need to be init'd
    Vector x_t; x.MakeColumnVector(t, &x_t);
    Vector y_t; y.MakeColumnVector(t, &y_t);
    propagate_one_step(lds.c_mat, x_t, v_t, &y_t);

    /*
      Perform measurement update */
    // Alg. takes (lds, y_t, x_{t|t-1}, p_{t|t-1}, inno_cov_{t|t-1})
    // outputs (x_{t|t}, p_{t|t}, k_gain{t})
    // Also make aliases which do not need to  be init'd
    Vector x_pred_t; x_pred.MakeColumnVector(t, &x_pred_t);
    Vector y_pred_t; y_pred.MakeColumnVector(t, &y_pred_t);
    Vector x_hat_t; x_hat.MakeColumnVector(t, &x_hat_t);
    KalmanFiltTimeInvariantMstUpdate(lds, y_t, x_pred_t, p_pred[t], 
                                     y_pred_t, inno_cov[t], x_hat_t, 
                                     p_hat[t], k_gain[t]);

    /* The system might want to do sth. here
       Ex. u_t = f(x_{t|t})
       Here, u_t is assumed to be a normally distributed vec.
       Also make aliases*/
    Vector u_t; 
    u.MakeColumnVector(t, &u_t);
    RandVector(u_t);


    /* Perform Time update 
     Alg. takes (lds, x_{t|t}, p_{t|t}, y_t, u_t)
     outputs (x_{t+1|t}, y_{t+1|t}, p_{t+1|t}, inno_cov{t+1|t}) 
     Also make aliases that do not need to be init'd. 
     But do so if not at final time step */
    if (t <  t_tot) { 
      Vector x_pred_t_next; x_pred.MakeColumnVector(t + 1, &x_pred_t_next);
      Vector y_pred_t_next; y_pred.MakeColumnVector(t + 1, &y_pred_t_next);
      KalmanFiltTimeInvariantTimeUpdate(lds, x_hat_t, p_hat[t], y_t, u_t, 
                                        x_pred_t_next, y_pred_t_next, 
                                        p_pred[t + 1], inno_cov[t + 1]);      
      
    };
    /* Propagate states 1-step forward 
       Again the true plant/ system is up to the user to define
       x_{t+1} = a_mat*x_{t} + b_mat*u_{t} + w_{t} 
       unless we are at the last time step */      
    if (t <  t_tot) { 
      Vector x_t_next; x.MakeColumnVector(t+1, &x_t_next);
      propagate_one_step(lds.a_mat, lds.b_mat, x_t, u_t, w_t, &x_t_next);
    };
  }; /* demo */
  
  
  /* We will store the results so that a similar Matlab
     implementation can be used as a comparison
     To save:
     w, v (stochastic realizations)
     u, x, y (actions)
     x_pred, p_pred[t_tot] (expect this to converge)
     x_hat, p_hat[t_tot] (expect this to converge)
     y_pred, inno_cov[t_tot] (expect this to converge)
     k_gain[t_tot] (expect this to converge)
     
     These will be saved as separate *.csv files
     See test_kf.m for comparison tests 
     and plotting capabilities */
  
  Matrix w_trans, v_trans; 
  Matrix u_trans, x_trans, y_trans; 
  Matrix x_pred_trans;
  Matrix x_hat_trans; 
  Matrix y_pred_trans; 
  Matrix k_gain_end_trans;
    
  la::TransposeInit(w, &w_trans);
  la::TransposeInit(v, &v_trans);
  la::TransposeInit(u, &u_trans);
  la::TransposeInit(x, &x_trans);
  la::TransposeInit(y, &y_trans);
  la::TransposeInit(x_pred, &x_pred_trans);
  la::TransposeInit(x_hat, &x_hat_trans);
  la::TransposeInit(y_pred, &y_pred_trans);
  la::TransposeInit(k_gain[t_tot], &k_gain_end_trans);
  
  data::Save("w_out", w_trans);
  data::Save("v_out", v_trans);
  data::Save("u_out", u_trans);
  data::Save("x_out", x_trans);
  data::Save("y_out", y_trans);
  data::Save("x_pred_out", x_pred_trans);
  data::Save("p_pred_end_out", p_pred[t_tot]);
  data::Save("x_hat_out", x_hat_trans);
  data::Save("p_hat_end_out", p_hat[t_tot]);
  data::Save("y_pred_out", y_pred_trans);
  data::Save("inno_cov_end_out", inno_cov[t_tot]);
  data::Save("k_gain_end_out", k_gain_end_trans);             
  
  fx_done(fx_root);
}; /* main */