Kalman.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$ 
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP 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 3 of the License, or
// (at your option) any later version.
//
// MIP 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 MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------



/* @{ */

#ifndef __KALMAN_H_
#define __KALMAN_H_

#include <stdlib.h>
#include <time.h>

#include <Types.h>
#include <fstream>

#include <S1.h>
#include <R2.h>
#include <Surface.h>
#include <MIPMatrix.h>

#define GRAV     9.81    // Gravity acceleration.
#define EPSILON                  0.01  // Maximum acceptable error in calculus.


#define A_ROW            3    // Number of rows in the A(k) matrix.
#define A_COL            3    // Number of columns in the A(k) matrix.
#define C_ROW              2    // Number of rows in the C(k) matrix. C_ROW must be equal to G_ROW, see function matrix_K
#define C_COL              3    // Number of columns in the C(k) matrix.
#define Pp_ROW     3    // Number of rows in the Pp(k) matrix.
#define Pp_COL     3    // Number of columns in the Pp(k) matrix.
#define K_ROW              3    // Number of rows in the K(k+1) matrix.
#define K_COL              2    // Number of columns in the K(k+1) matrix.
#define P_ROW                    3    // Number of rows in the P(k+1) matrix.
#define P_COL                    3    // Number of columns in the P(k+1) matrix.
#define F_ROW                  3    // Number of rows in the F(k) matrix.
#define F_COL                  3    // Number of columns in the F(k) matrix.
#define G_ROW              2    // Number of rows in the G(k+1) matrix. G_ROW must be equal to C_ROW, see function matrix_K
#define G_COL              2    // Number of columns in the G(k+1) matrix.
#define OUT_PRED_ROWS      2    // Number of rows in the OutPred(k+1) matrix.
#define GPS_MEAS              2    // GPS vector data.
#define ENC_MEAS   3    // Encoder measures used as measured inputs to calculate robot status prediction X(k+1,k).

/*
Nel filtro di Kalman ci sono 4 variabili fondamentali:
1:- La stima dello stato al passo k fatta utilizzando le misure fino al passo k. Tale stima è indicata con X(k|k)
2:- La stima dello stato al passo k+1 fatta utilizzando le misure fino al passo k. Tale stima è indicata con X(k+1|k).
3:- La stima della uscita al passo k fatta utilizzando le misure fino al passo k. Tale stima è indicata con Y(k|k) e  coincide con la misura stessa 
    effettuata al passo k cioè Y(k).
4:- La stima della uscita al passo k+1 fatta utilizzando le misure fino al passo k. Tale stima è indicata con Y(k+1|k).

Dato il modello matematico del robot nella forma:
X(k+1)=f(x(k),u(k),k) -- Equazione di stato
Y(k)=h(x(k),u(k),k)   -- Equazione di uscita

Le stime indicate di sopra ai passi 2 e 4 si fanno utilizzando, rispettivamente, le equazioni di stato e di uscita del modello matematico del robot.
*/
#include <boost/concept_check.hpp>

class KalmanFilter {
 private:
    Decimal  CycleTime;//Kalman filter sampling time in seconds.

  MIPMatrix *State_0;   //Estimate of robot status at time k: X(k|k). 3 data: x,y,theta.
  MIPMatrix *State_1;   //Estimate of robot status at time k+1: X(k+1|k). 3 data: x,y,theta.
  MIPMatrix *OutPred;   //Prediction of robot output at time k+1: Y(k+1|k). 3 data: x,y,theta.
  MIPMatrix *GPS_Meas;  //GPS measures at time k: Y(k|k); 2 data: x,y.
  MIPMatrix *ENC_Meas;  //Encoder measures at time k: used to calculate the robot status prediction X(k+1,k).
  
  Decimal RobotMass;//Mass of the robot.
  Decimal RobotMoI; //Moment of Inertia of the robot (yaw axis).
  
  Surface *Surf;         //Surface on which the robot is moving
  int CurrentRobotPlane; // Reference to the current plane where the robot is moving.
  int SemiSpace; //Side w.r.t. the next plane. Used to detect the change of the plane where the robot is moving.
  
  // Current robot distances from reference points  
  //Decimal CurRefPointDist;//Robot distance from reference point in the current surface plane
  //Decimal NextRefPointDist;//Robot distance from reference point in the next surface plane
  //Decimal PrevRefPointDist;//Robot distance from reference point in the previous surface plane
  
  MIPMatrix *Mat_Pp;
  MIPMatrix *Mat_A;  
  MIPMatrix *Mat_C;
  MIPMatrix *Mat_K;
  MIPMatrix *Mat_P;
  MIPMatrix *Mat_G;
  MIPMatrix *Mat_F;

  MIPMatrix *tmpmat;//Only for matrix functions test
  
  Decimal Force;//Force applied to the robot.
  Decimal Torque;//Torque applied to the robot.
  bool GPS_MeasAvail;//1 = GPS measure available; 0 = GPS measures not available;
  
  void initializeMatrixC();
  
  void initializeState_1();//This function initializes the robot predicted status matrix: State_1(k+1|k).

  void initializeMatrixF();//This function initializes the robot status noise matrix.
  
                void initializeMatrixG();//This function initializes the robot output noise matrix.

  void initializeMatrixPp();//This function initializes the equivalent of the covariance error prediction matrix.

  void setSemiSpace();//This function sets a reference value (+1 or -1) to tag the semi-space where the robot is moving.
                      //The reference plane is the next w.r.t the one on which the robot is moving.
  
  void updRobotPlane();//This function updates the plane on which the robot is moving
  
  // The following functions implements the Kalman filter steps
  
  void outPred();//II: This function calculates the output prediction.
  
  //void Matrix_C();//III: This matrix is the Jacobian matrix of h() in X(k+1,k). It is 
                    // a (2x3) matrix and is built by the EKF constructor.
  
  void matrix_K();//IV: This function calculates the Kalman gain matrix K(k+1).
  
  void matrix_P();//V: This function calculates the matrix P(k+1).

  void estimateRobotStatus();//VI: This function estimates the matrix X(k+1,k+1) using GPS measures.

  void calculateStatusPrediction();//VIII: This function calculates the prediction of the status of the robot X(k+1|k)

  void calculate_A();//IX: This function calculates the Jacobian matrix of f() in X(k,k).

  void calculatePp();//X:  This function calculates the Pp(k+1) matrix
    
  //void Get_GPS_Measures();
 
       
 public:
  KalmanFilter (string namefile)
  {
          cout << "KalmanFilter::KalmanFilter() starting" << "\n";
   //tmpmat = new MIPMatrix(3,3,ZERO_MATRIX); //Used only for matrix functions test.
   State_0 = new MIPMatrix(A_ROW,1,ZERO_MATRIX);     
   State_1 = new MIPMatrix(A_ROW,1,ZERO_MATRIX);
          //cout << "KalmanFilter::KalmanFilter() starting 2" << "\n";   
   OutPred = new MIPMatrix(OUT_PRED_ROWS,1,ZERO_MATRIX);
   GPS_Meas = new MIPMatrix(GPS_MEAS,1,ZERO_MATRIX);
   ENC_Meas = new MIPMatrix(ENC_MEAS,1,ZERO_MATRIX);
          //cout << "KalmanFilter::KalmanFilter() starting 3" << "\n";   
   Mat_Pp = new MIPMatrix(Pp_ROW,Pp_COL,IDENTITY_MATRIX);     
   Mat_A = new MIPMatrix(A_ROW,A_COL,IDENTITY_MATRIX);
   //cout << "KalmanFilter::KalmanFilter() starting 4" << "\n";
   Mat_C = new MIPMatrix(C_ROW,C_COL,ZERO_MATRIX);
          //cout << "KalmanFilter::KalmanFilter() starting 4a" << "\n";
   Mat_K = new MIPMatrix(K_ROW,K_COL,ZERO_MATRIX); 
          //cout << "KalmanFilter::KalmanFilter() starting 5" << "\n";
   Mat_P = new MIPMatrix(P_ROW,P_COL,ZERO_MATRIX);       
   Mat_G = new MIPMatrix(G_ROW,G_COL,ZERO_MATRIX);
   Mat_F = new MIPMatrix(F_ROW,F_COL,ZERO_MATRIX);
          //cout << "KalmanFilter::KalmanFilter() starting 6" << "\n";
   cout << "Kalman Surface construction" << "\n";
   Surf = new Surface(namefile);
   
   CycleTime = 0.1;
   Force = 0;
   Torque = 0;
   
   cout << "Matrix_C" << endl;
   initializeMatrixC();//In this particular case mat_C a constant matrix and is initialized at startup.
                        Mat_C->printMIPMatrix();
                        cout << "\n";

   //cout << "Initialize F matrix" << endl;
   initializeMatrixF();
                        //Mat_F->printMIPMatrix();
                        cout << "\n";

   //cout << "Initialize Matrix G" << endl;
   initializeMatrixG();
                        //Mat_G->printMIPMatrix();
                        cout << "\n";

   cout << "Initialize status prediction matrix: State_1." << endl;
   initializeState_1();//This function initializes the robot predicted status matrix: State_1(k+1|k). Initial conditions.
                        State_1->printMIPMatrix();
                        cout << "\n";

   cout << "Initialize Matrix Pp" << endl;
   initializeMatrixPp();
                        Mat_Pp->printMIPMatrix();
                        cout << "\n";

   CurrentRobotPlane = 0;// It is assumed that the robot initial position belongs to the first plane.
   SemiSpace = 0;
   setSemiSpace();
   unsetGPSAvailability(); 
   cout << "KalmanFilter Constructor OK" << endl;
  }

  ~KalmanFilter ()
  {
   delete State_0;
   delete State_1;
//    delete OutPred_GPS;
   delete OutPred;
   delete GPS_Meas;
   delete ENC_Meas;
   delete Mat_Pp;     
   delete Mat_A;          
   delete Mat_C;
   delete Mat_K; 
   delete Mat_P;       
   delete Mat_G;
   delete Mat_F;
   delete Surf;
   cout << "KalmanFilter destructor OK" << endl;
  }
  
  void setAppliedForce (Decimal F)
  {
   Force = F;
   return;
  }

  void setAppliedTorque (Decimal To)
  {
      Torque = To;
      return;
  }

  void setCycleTime (Decimal T)
  {
      CycleTime = T;
      return;
  }

  void setCurrentRobotPlane (int Plane)
  {
      CurrentRobotPlane = Plane;
      return;
  }
  
  void setGPSAvailability()
  {
      GPS_MeasAvail = true;
  }
  
  void unsetGPSAvailability()
  {
   GPS_MeasAvail = false;
  }
  
  Decimal getAppliedForce(){return Force;}
  
  Decimal getAppliedTorque(){return Torque;}

  Decimal getCycleTime(){return CycleTime;}

  int getCurrentRobotPlane(){return CurrentRobotPlane;}
  
  int getGPSAvailability(){return GPS_MeasAvail;}
  
  MIPMatrix* getRobStatus(){return State_0;}
  

  bool TranslateGPSMeasures(Position3D &pos)
  {
      // pos = Received GPS measures expressed in the Inertial Reference Frame to be translated in the Local Reference Frame.
      MIPMatrix *GPS_MeasInert;//GPS measures expressed in the inertial reference frame.
      MIPMatrix *GPS_Measlocal;//GPS measures expressed in the local reference frame.
      MIPMatrix *InertialToLocal;//Rototranslation matrix from Inertial reference frame to local reference frame.
      
      MIPMatrix *tmp;

      Decimal x_loc;
      Decimal y_loc;
      Decimal z_loc;
    
      GPS_MeasInert = new MIPMatrix(4,1,ZERO_MATRIX);
      GPS_Measlocal = new MIPMatrix(4,1,ZERO_MATRIX);
      InertialToLocal = new MIPMatrix(4,4,ZERO_MATRIX);
      
      tmp = new MIPMatrix(4,4,ZERO_MATRIX);
      
      GPS_MeasInert->SetMIPMatrixVal(1,1,pos.x());
      GPS_MeasInert->SetMIPMatrixVal(2,1,pos.y());
      GPS_MeasInert->SetMIPMatrixVal(3,1,pos.z());
      GPS_MeasInert->SetMIPMatrixVal(4,1,1);
      
      cout << "Kalman Filter:setGPSMeasures(): GPS data in the inertial reference frame:" << endl;
      GPS_MeasInert->printMIPMatrix();
      cout << "\n";      

  //  cout << "Pause 2 sec" << "\n";
      
      tmp->operator=(*(Surf->GetRotTranslMatrix(CurrentRobotPlane)));
  //  tmp->printMIPMatrix();

      if(InertialToLocal->inv44(*tmp) == 0)
      {
   cout << "Kalman Filter::setGPSMeasures():   Invalid GPS measures." << "\n";
   return false;
      }
      
      cout << "Kalman Filter:setGPSMeasures(): Rototranslation Matrix from inertial to local reference frame:" << endl;
      InertialToLocal->printMIPMatrix();
      cout << "\n";
      
      GPS_Measlocal->mul(*InertialToLocal,*GPS_MeasInert);
      
      x_loc = GPS_Measlocal->GetMIPMatrixVal(1,1);
      y_loc = GPS_Measlocal->GetMIPMatrixVal(2,1);
      z_loc = GPS_Measlocal->GetMIPMatrixVal(3,1);
      
      delete GPS_MeasInert;
      delete GPS_Measlocal;
      delete InertialToLocal;
      
      if((z_loc < -EPSILON) || (z_loc > EPSILON))//z_loc != 0. EPSILON=0.01
      {
   cout << "Wrong data received from GPS. Error too big for z_loc = " << z_loc << " it should be zero!!" << endl;
   //return false;
      }
       
      //Write the GPS measure in the input matrix of the Kalman Filter.
      GPS_Meas->SetMIPMatrixVal(1,1,x_loc);
      GPS_Meas->SetMIPMatrixVal(2,1,y_loc);
      cout << "Kalman Filter:setGPSMeasures(): GPS data in the local reference frame:" << endl;
      GPS_Meas->printMIPMatrix();
      cout << "\n";
      cout << "Kalman Filter:setGPSMeasures(): GPS z_loc= " << z_loc << endl;

      return true;
  }


  void processENCMeasures(Decimal d_x, Decimal d_y, Angle d_theta)

  {
//       Decimal meas_speed;
//       Decimal meas_turnrate;
      Angle  newtheta;
      Angle  oldtheta;
      Decimal dx_loc;
      Decimal dy_loc;
      Decimal Noise;
      Angle  *Noise_1;
      
      MIPMatrix *ds_Iner;
      MIPMatrix *ds_Loc;
      MIPMatrix *RotLoc2Iner;//Rotation matrix from local reference frame to inertial reference frame
      MIPMatrix *RotIner2Loc;//Rotation matrix from inertial reference frame to local reference frame
      
      // d_x,d_y,d_theta are expressed in the global reference frame, while I need the corresponding values
      // in the local reference frame. This function calculates the required values taking into consideration that the robot measured
      // speed is the same in both reference frames, because the relative speed of the local reference frame w.r.t the inertial
      // reference frame is zero.
      
      cout << "KalmanFilter::getENCMeasures() (without noise) (d_x=" << d_x << ", d_y=" << d_y << ", d_theta=" << d_theta.dCastDeg() << ")\n";
      
      // Add noise to measures d_x, d_y, d_theta
      srand(time(NULL));
      Noise=(drand48() - 0.5)*0.01;
      d_x = d_x + Noise;
      cout << "KalmanFilter::getENCMeasures()  d_x Noise=" << Noise << "\n";
      srand(time(NULL));
      Noise=(drand48() - 0.5)*0.01;
      d_y = d_y + Noise;
      cout << "KalmanFilter::getENCMeasures()  d_y Noise=" << Noise << "\n";        
      srand(time(NULL));
      Noise = (drand48() - 0.5)*0.01;
      cout << "KalmanFilter::getENCMeasures()  d_theta Noise=" << Noise << "\n";
      d_theta.operator=(d_theta.dCastDeg() + Noise);
      cout << "KalmanFilter::getENCMeasures() with noise (d_x=" << d_x << ", d_y=" << d_y << ", d_theta=" << 
       d_theta.dCast2Pi() << " = " << d_theta.dCastDeg() << ")\n";
      
      //Robot movement in the inertial reference frame. 
      ds_Iner = new MIPMatrix(3,1,ZERO_MATRIX);
      ds_Iner->SetMIPMatrixVal(1,1,d_x);
      ds_Iner->SetMIPMatrixVal(2,1,d_y);
      ds_Iner->SetMIPMatrixVal(3,1,0);//d_z=0
      cout << "KalmanFilter::outEstim_ENC(), ds_Iner vector components: " << "\n";
      cout << "dx_Iner= " << ds_Iner->GetMIPMatrixVal(1,1) << "\n";
      cout << "dy_Iner= " << ds_Iner->GetMIPMatrixVal(2,1) << "\n";
      cout << "dz_Iner= " << ds_Iner->GetMIPMatrixVal(3,1) << "\n";
       cout << "d_theta= " << d_theta.dCastDeg() << "\n";
      
      RotLoc2Iner = new MIPMatrix(3,3,ZERO_MATRIX);
      RotIner2Loc = new MIPMatrix(3,3,ZERO_MATRIX);
      
      //Get rotation matrix from local to inertial reference frame
         cout << "KalmanFilter::getENCMeasures(), RotTranslMatrix matrix: " << "\n";
      (Surf->GetRotTranslMatrix(CurrentRobotPlane))->printMIPMatrix();
      RotLoc2Iner->MinorMatrix(*(Surf->GetRotTranslMatrix(CurrentRobotPlane)),4,4);
      cout << "KalmanFilter::getENCMeasures(), RotLoc2Iner matrix: " << "\n";
      RotLoc2Iner->printMIPMatrix();
      RotIner2Loc->inv33(*RotLoc2Iner);
      cout << "KalmanFilter::getENCMeasures(), RotIner2Loc matrix: " << "\n";
      RotIner2Loc->printMIPMatrix();
      
      //Robot movement in the local reference frame.
      ds_Loc = new MIPMatrix(3,1,ZERO_MATRIX);      
      ds_Loc->mul(*RotIner2Loc,*ds_Iner);
      dx_loc = ds_Loc->GetMIPMatrixVal(1,1);
      dy_loc = ds_Loc->GetMIPMatrixVal(2,1);
      cout << "KalmanFilter::getENCMeasures(), ds_Loc vector components: " << "\n";
      cout << "dx_loc= " << dx_loc << "\n";
      cout << "dy_loc= " << dy_loc << "\n";
      cout << "dz_loc= " << ds_Loc->GetMIPMatrixVal(3,1) << "\n";
      cout << "d_theta= " << d_theta.dCastDeg() << "\n";
      
      if(ds_Loc->GetMIPMatrixVal(3,1) != 0.0)
         cout << "KalmanFilter::getENCMeasures(): ERROR - dz_loc <> 0!!! " << ds_Loc->GetMIPMatrixVal(3,1) << "\n";
            
      newtheta.operator=(d_theta);
 
      ENC_Meas->SetMIPMatrixVal(1,1,dx_loc);
      ENC_Meas->SetMIPMatrixVal(2,1,dy_loc);
      ENC_Meas->SetMIPMatrixVal(3,1,newtheta.dCast2Pi());
//       ENC_Meas->SetMIPMatrixVal(4,1,meas_speed);
//       ENC_Meas->SetMIPMatrixVal(5,1,meas_turnrate);
// 
      //cout << "KalmanFilter::getENCMeasures().Robot status prediction calculated using ENC measures as if they were inputs: " << endl;
      cout << "KalmanFilter::getENCMeasures(), ds_Loc vector components: " << "\n";
      cout << "dx_loc= " << ENC_Meas->GetMIPMatrixVal(1,1) << "\n";
      cout << "dy_loc= " << ENC_Meas->GetMIPMatrixVal(2,1) << "\n";
      cout << "newtheta= " << ENC_Meas->GetMIPMatrixVal(3,1) << "\n";
      cout << "KalmanFilter::getENCMeasures() (d_theta): " << newtheta.dCastDeg() << "\n";

 
      delete RotLoc2Iner;
      delete RotIner2Loc;
      delete ds_Iner;
      delete ds_Loc;
      return;
  }
  
  
  void PositionErrorEstimate(Position3D &pos)//This function calculates position error estimate.
  {
          MIPMatrix *LoclRobPos;//GPS measures expressed in the local reference frame.
      MIPMatrix *InerRobPos;//GPS measures expressed in the inertial reference frame.
                    
      Decimal X_err;
      Decimal Y_err;
      Decimal Z_err;
      
      cout << "KalmanFilter::PositionErrorEstimate()" << "\n";
      LoclRobPos = new MIPMatrix(4,1,ZERO_MATRIX);
      InerRobPos = new MIPMatrix(4,1,ZERO_MATRIX);
      
      //Estimated Robot position in the local reference frame. z_loc = 0; Component (4,1) of the vector is 1 for rototranslation matrix.
      LoclRobPos->SetMIPMatrixVal(1,1,State_0->GetMIPMatrixVal(1,1));
      LoclRobPos->SetMIPMatrixVal(2,1,State_0->GetMIPMatrixVal(2,1));
      LoclRobPos->SetMIPMatrixVal(3,1,0);
      LoclRobPos->SetMIPMatrixVal(4,1,1);
      
      //Estimated Robot position in the inertial reference frame.
      InerRobPos->mul(*(Surf->GetRotTranslMatrix(CurrentRobotPlane)),*LoclRobPos);
      
      //Kalman Filtering position error.
      cout << "KalmanFilter::PositionErrorEstimate(): Robot position from proxi interface." << "\n";
          cout << "X_pos (proxi) = " << pos.x() << "\n";
      cout << "Y_pos (proxi) = " << pos.y() << "\n";
      cout << "Z_pos (proxi) = " << pos.z() << "\n";

      cout << "KalmanFilter::PositionErrorEstimate(): Estimated Robot position." << "\n";
          cout << "X_pos (estimated) = " << InerRobPos->GetMIPMatrixVal(1,1) << "\n";
      cout << "Y_pos (estimated) = " << InerRobPos->GetMIPMatrixVal(2,1) << "\n";
      cout << "Z_pos (estimated) = " << InerRobPos->GetMIPMatrixVal(3,1) << "\n";
      
          cout << "KalmanFilter::PositionErrorEstimate(): Estimated position error." << "\n";
      cout << "X_err = " << pos.x()-InerRobPos->GetMIPMatrixVal(1,1) << "\n";
      cout << "Y_err = " << pos.y()-InerRobPos->GetMIPMatrixVal(2,1) << "\n";
      cout << "Z_err = " << pos.z()-InerRobPos->GetMIPMatrixVal(3,1) << "\n";

      //Predicted Robot position in the local reference frame. z_loc = 0; Component (4,1) of the vector is 1 for rototranslation matrix.
      LoclRobPos->SetMIPMatrixVal(1,1,State_1->GetMIPMatrixVal(1,1));
      LoclRobPos->SetMIPMatrixVal(2,1,State_1->GetMIPMatrixVal(2,1));
      LoclRobPos->SetMIPMatrixVal(3,1,0);
      LoclRobPos->SetMIPMatrixVal(4,1,1);
      
      //Predicted Robot position in the inertial reference frame.
      InerRobPos->mul(*(Surf->GetRotTranslMatrix(CurrentRobotPlane)),*LoclRobPos);

      cout << "KalmanFilter::PositionErrorEstimate(): Predicted Robot position." << "\n";
          cout << "X_pos (estimated) = " << InerRobPos->GetMIPMatrixVal(1,1) << "\n";
      cout << "Y_pos (estimated) = " << InerRobPos->GetMIPMatrixVal(2,1) << "\n";
      cout << "Z_pos (estimated) = " << InerRobPos->GetMIPMatrixVal(3,1) << "\n";
      
          cout << "KalmanFilter::PositionErrorEstimate(): Predicted position error." << "\n";
      cout << "X_err = " << pos.x()-InerRobPos->GetMIPMatrixVal(1,1) << "\n";
      cout << "Y_err = " << pos.y()-InerRobPos->GetMIPMatrixVal(2,1) << "\n";
      cout << "Z_err = " << pos.z()-InerRobPos->GetMIPMatrixVal(3,1) << "\n";

      return;
  }
  
  
  void updateKalmanFilter()//Kalman filter algorithm implementation
  {
          Decimal tmp;
   Angle tmp_1;
   
          cout << "updateKalmanFilter() started." << endl;
   //Initial conditions already set at Kalman Filter instantiation. (Status prediction initialization and Pp matrix initialization). 
   //See EKF constructor. //I
   cout << "Previous estimated state: x= " << State_0->GetMIPMatrixVal(1,1) << ", y= " << State_0->GetMIPMatrixVal(2,1)
                                   << ", theta= " << State_0->GetMIPMatrixVal(3,1) << "\n";
   cout << "Previous predicted state: x= " << State_1->GetMIPMatrixVal(1,1) << ", y= " << State_1->GetMIPMatrixVal(2,1)
                                   << ", theta= " << State_1->GetMIPMatrixVal(3,1) << "\n";
   cout << "Encoder measures: dx_loc= " << ENC_Meas->GetMIPMatrixVal(1,1) << ", dy_loc= " << ENC_Meas->GetMIPMatrixVal(2,1)
                                   << ", dtheta= " << ENC_Meas->GetMIPMatrixVal(3,1) << "\n";
 
   if(GPS_MeasAvail == true)
   {
      cout << "updateKalmanFilter() - GPS measure available." << endl;
      
      outPred();//II: This function calculates the prediction of the output Y(k+1|k). 
      
      //Matrix_C();//III: Since this is a constant matrix it is calculated only once at EKF initialization.
      
      matrix_K();//IV: This function calculates the Kalman gain matrix K(k+1). 
     
             matrix_P();//V: This function calculates the matrix P(k+1).
     
      //cout << "updateKalmanFilter() estimating robot status using GPS measures." << endl;
      estimateRobotStatus();//VI: This function estimates the matrix X(k+1,k+1).
      
      calculateStatusPrediction();//VIII: This function calculates the prediction of the status of the robot X(k+1|k)=f(-)
      
      calculate_A();//IX: This function calculates the Jacobian matrix of f() in X(k,k).
      
      calculatePp();//X: This function calculates the Pp(k+1) matrix.      

      updRobotPlane();//This function calculates the plane on which the robot is moving.
      
      unsetGPSAvailability();
   }
   else
   {
      //GPS measures not available, execute odometry integration to predict the new robot status X(k+1,k) //VIII
      tmp = State_1->GetMIPMatrixVal(1,1) + ENC_Meas->GetMIPMatrixVal(1,1);
      State_1->SetMIPMatrixVal(1, 1, tmp);
      tmp = State_1->GetMIPMatrixVal(2,1) + ENC_Meas->GetMIPMatrixVal(2,1);
      State_1->SetMIPMatrixVal(2, 1, tmp);
      tmp_1.operator=(State_1->GetMIPMatrixVal(3,1) + ENC_Meas->GetMIPMatrixVal(3,1));
      State_1->SetMIPMatrixVal(3, 1, tmp_1.dCast2Pi());
      //State_1->SetMIPMatrixVal(4, 1, ); 
      //State_1->SetMIPMatrixVal(5, 1, );
   }   
   cout << "Last estimated state: x= " << State_0->GetMIPMatrixVal(1,1) << ", y= " << State_0->GetMIPMatrixVal(2,1)
                                   << ", theta= " << State_0->GetMIPMatrixVal(3,1) << "\n";
   cout << "Last predicted state: x= " << State_1->GetMIPMatrixVal(1,1) << ", y= " << State_1->GetMIPMatrixVal(2,1)
                                   << ", theta= " << State_1->GetMIPMatrixVal(3,1) << "\n";

   return;
  }
};
#endif