KalmanImu.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: franchi@diag.uniroma1.it stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------



/* @{ */

#ifndef __KALMAN_IMU_H_
#define __KALMAN_IMU_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_IMU            5    // Number of rows in the A(k) matrix.
#define A_COL_IMU            5    // Number of columns in the A(k) matrix.
#define C_ROW_IMU               5    // Number of rows in the C(k) matrix. C_ROW must be equal to G_ROW, see function matrix_K
#define C_COL_IMU               5    // Number of columns in the C(k) matrix.
#define Pp_ROW_IMU     5    // Number of rows in the Pp(k) matrix.
#define Pp_COL_IMU     5    // Number of columns in the Pp(k) matrix.
#define K_ROW_IMU               5    // Number of rows in the K(k+1) matrix.
#define K_COL_IMU               5    // Number of columns in the K(k+1) matrix.
#define P_ROW_IMU                   5    // Number of rows in the P(k+1) matrix.
#define P_COL_IMU                    5    // Number of columns in the P(k+1) matrix.
#define F_ROW_IMU                  5    // Number of rows in the F(k) matrix.
#define F_COL_IMU                  5    // Number of columns in the F(k) matrix.
#define G_ROW_IMU               5    // Number of rows in the G(k+1) matrix. G_ROW must be equal to C_ROW, see function matrix_K
#define G_COL_IMU               5    // Number of columns in the G(k+1) matrix.
#define OUT_PRED_ROWS_IMU       5    // Number of rows in the OutPred(k+1) matrix.
#define OUT_MEAS_ROWS_IMU       5    // Number of rows in the OutMeas(k+1) matrix.
#define GPS_MEAS_IMU              2    // GPS data.
#define ENC_MEAS_IMU    5    // Encoder data.
#define ACC_MEAS_IMU    3    // Accelerometer data.
#define GYRO_MEAS_IMU    3    // Gyro data.
#define MAGN_MEAS_IMU    3    // Magnetometer data.

/*
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 KalmanFilterImu
{
 private:
  Decimal  CycleTime;   //Kalman filter sampling time in seconds.
  Decimal  Force;       //Force applied to the robot
  Decimal  Torque;      //Torque applied to the robot
  
  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 *OutMeas;   //Estimation of robot output at time k+1: 
  
  // GPS Measures
  MIPMatrix *GPS_Meas;  //GPS measures at time k: Y(k|k); 2 data: x,y.
  Decimal    GPS_MeasTime;  //Time at GPS measures read
  Decimal    GPS_DeltaTime;
  
  // Encoder Measures
  MIPMatrix *ENC_Meas;      //Encoder measures at time k.
  Decimal    ENC_MeasTime;  //Time at encoder measures read
  Decimal    ENC_DeltaTime;
  
  // IMU Measures
  MIPMatrix *ACC_Meas;  //Accelerometer measures at time k.
  MIPMatrix *GYRO_Meas;  //Gyro measures at time k.
  MIPMatrix *MAGN_Meas;  //Magnetometer measures at time k.
  Decimal    IMU_MeasTime;  //Time at IMU measures read
  Decimal    IMU_DeltaTime;
  
  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 distance 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(); I(5x5) matrix initialized at its instantiation
  
  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:
  KalmanFilterImu (string namefile)
  {
          cout << "KalmanFilterImu::KalmanFilterImu() starting" << "\n";
   //tmpmat = new MIPMatrix(3,3,ZERO_MATRIX); //Used only for matrix functions test.
   State_0 = new MIPMatrix(A_ROW_IMU,1,ZERO_MATRIX);     
   State_1 = new MIPMatrix(A_ROW_IMU,1,ZERO_MATRIX);
   
          //cout << "KalmanFilterImu::KalmanFilterImu() starting 2" << "\n";   
   OutPred = new MIPMatrix(OUT_PRED_ROWS_IMU,1,ZERO_MATRIX);
   OutMeas = new MIPMatrix(OUT_MEAS_ROWS_IMU,1,ZERO_MATRIX);
   
   ENC_Meas = new MIPMatrix(ENC_MEAS_IMU,1,ZERO_MATRIX);  //Encoder measures at time k.
   ENC_MeasTime = 0;
   ENC_DeltaTime = 0; 
   
   ACC_Meas = new MIPMatrix(ACC_MEAS_IMU,1,ZERO_MATRIX);  //Accelerometer measures at time k.
   GYRO_Meas = new MIPMatrix(GYRO_MEAS_IMU,1,ZERO_MATRIX); //Gyro measures at time k.
   MAGN_Meas = new MIPMatrix(MAGN_MEAS_IMU,1,ZERO_MATRIX); //Magnetometer measures at time k.
   IMU_MeasTime = 0;
   IMU_DeltaTime = 0; 

   GPS_Meas = new MIPMatrix(GPS_MEAS_IMU,1,ZERO_MATRIX);  //GPS measures at time k.
   GPS_MeasTime = 0;
   GPS_DeltaTime = 0; 

          //cout << "KalmanFilterImu::KalmanFilterImu() starting 3" << "\n";   
   Mat_Pp = new MIPMatrix(Pp_ROW_IMU, Pp_COL_IMU,IDENTITY_MATRIX);     
   Mat_A = new MIPMatrix(A_ROW_IMU, A_COL_IMU,IDENTITY_MATRIX);
   //cout << "KalmanFilterImu::KalmanFilterImu() starting 4" << "\n";
   Mat_C = new MIPMatrix(C_ROW_IMU, C_COL_IMU,IDENTITY_MATRIX);
          //cout << "KalmanFilterImu::KalmanFilterImu() starting 4a" << "\n";
   Mat_K = new MIPMatrix(K_ROW_IMU, K_COL_IMU,ZERO_MATRIX); 
          //cout << "KalmanFilterImu::KalmanFilterImu() starting 5" << "\n";
   Mat_P = new MIPMatrix(P_ROW_IMU, P_COL_IMU,ZERO_MATRIX);       
   Mat_G = new MIPMatrix(G_ROW_IMU, G_COL_IMU,ZERO_MATRIX);
   Mat_F = new MIPMatrix(F_ROW_IMU, F_COL_IMU,ZERO_MATRIX);

   CycleTime = 0;
   Force = 0;
   Torque = 0;

          //cout << "KalmanFilterImu::KalmanFilterImu() starting 6" << "\n";
   cout << "Kalman Surface construction" << "\n";
   Surf = new Surface(namefile);
   
   cout << "KalmanFilterImu::Matrix_C" << endl;
   //initializeMatrixC();//In this particular case mat_C  is a constant matrix and it's 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 << "KalmanFilterImu:: Initialize status prediction matrix: State_1." << endl;
   initializeState_1();//This function initializes the robot predicted status matrix: State_1(k+1|k). Initial conditions.
                        cout << "\n";

   initializeMatrixPp();
                        cout << "\n";

   CurrentRobotPlane = 0;// It is assumed that the robot initial position belongs to the first plane.
   SemiSpace = 0;
   setSemiSpace();
   GPS_MeasAvail = false; 
   cout << "KalmanFilterImu:: Constructor end." << endl;  }

  ~KalmanFilterImu()
  {
   cout << "KalmanFilterImu destructor starting." << endl;
   delete State_0;
   delete State_1;
   
   delete OutPred;
   delete OutMeas;
   
   delete GPS_Meas;
   delete ENC_Meas;
   delete ACC_Meas;
   delete GYRO_Meas;
   delete MAGN_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 << "KalmanFilterImu 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;
      return;
  }
  
  void unsetGPSAvailability()
  {
      GPS_MeasAvail = false;
      return;
  }
  
  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;}
  
  void updateENCMeasuresTime(Decimal Time)
  {
      ENC_DeltaTime = Time - ENC_MeasTime; 
      cout << "KalmanFilterImu::updateENCMeasuresTime(). previous read time = " << ENC_MeasTime << "; Last read time = " << Time << "; Delta time = " << ENC_DeltaTime << "\n";
      ENC_MeasTime = Time;
      return;
  }

  void updateIMUMeasuresTime(Decimal Time)
  {
      IMU_DeltaTime = Time - IMU_MeasTime; 
      cout << "KalmanFilterImu::updateIMUMeasuresTime(). previous read time = " << IMU_MeasTime << "; Last read time = " << Time << "; Delta time = " << IMU_DeltaTime << "\n";
      IMU_MeasTime = Time;
      return;
  }

  void updateGPSMeasuresTime(Decimal Time)
  {
      GPS_DeltaTime = Time - GPS_MeasTime; 
      cout << "KalmanFilterImu::updateGPSMeasuresTime(). previous read time = " << GPS_MeasTime << "; Last read time = " << Time << "; Delta time = " << GPS_DeltaTime << "\n";
      GPS_MeasTime = Time;
      return;
  }

  void ProcessENCMeasures(MIPMatrix *RobStat)
  {
      Decimal meas_speed;
      Decimal meas_turnrate;
      
      Decimal dx_loc;
      Decimal dy_loc;
      Decimal Noise;
      Angle   Noise_1;
      Decimal tmp;
      Angle   tmp_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
      
      cout << "KalmanFilterImu::ProcessENCMeasures() (without noise) X = "    << RobStat->GetMIPMatrixVal(1,1) << "\n";
      cout << "KalmanFilterImu::ProcessENCMeasures() (without noise) Y = "    << RobStat->GetMIPMatrixVal(2,1) << "\n";
      cout << "KalmanFilterImu::ProcessENCMeasures() (without noise) Th = "   << RobStat->GetMIPMatrixVal(3,1) << "\n";
      cout << "KalmanFilterImu::ProcessENCMeasures() (without noise) ds = "   << RobStat->GetMIPMatrixVal(4,1) << "\n";
      cout << "KalmanFilterImu::ProcessENCMeasures() (without noise) dTh = "  << RobStat->GetMIPMatrixVal(5,1) << "\n";
      
      // Add noise to X measure
      tmp = RobStat->GetMIPMatrixVal(1,1);
      if(tmp == 0)
      {
   cout << "KalmanFilterImu::ProcessENCMeasures()  X = 0" << "\n";
   srand(time(NULL));
   Noise=(drand48() - 0.5)*0.001;
   RobStat->SetMIPMatrixVal(1,1,Noise); 
   cout << "KalmanFilterImu::ProcessENCMeasures()  X Noise=" << Noise << "\n";
      }
      else
      {
   srand(time(NULL));
   Noise=(drand48() - 0.5)*0.01*tmp;
   tmp = tmp + Noise;
   RobStat->SetMIPMatrixVal(1,1,tmp + Noise);
   cout << "KalmanFilterImu::ProcessENCMeasures()  X Noise=" << Noise << "\n";
      }

      // Add noise to Y measure
      tmp = RobStat->GetMIPMatrixVal(2,1);
      if(tmp == 0)
      {
   cout << "KalmanFilterImu::ProcessENCMeasures()  Y = 0" << "\n";
   srand(time(NULL));
   Noise=(drand48() - 0.5)*0.001;
   RobStat->SetMIPMatrixVal(2,1,Noise);
   cout << "KalmanFilterImu::ProcessENCMeasures() Y Noise=" << Noise << "\n";
      }
      else
      {
   srand(time(NULL));
   Noise=(drand48() - 0.5)*0.01*tmp;
   tmp = tmp + Noise;
   cout << "KalmanFilterImu::ProcessENCMeasures()  Y Noise=" << Noise << "\n";  
   RobStat->SetMIPMatrixVal(2,1,tmp + Noise);
      }

      // Add noise to theta measure
      tmp_1 = RobStat->GetMIPMatrixVal(3,1);
      if(tmp_1.dCast2Pi() == 0)
      {
          cout << "KalmanFilterImu::ProcessENCMeasures()  theta = 0" << "\n";
   srand(time(NULL));
   Noise_1 = Angle(drand48() - 0.5)*0.01;//Circa mezzo grado di errore
   RobStat->SetMIPMatrixVal(3,1,Noise_1.dCast2Pi());
   cout << "KalmanFilterImu::ProcessENCMeasures()  _theta Noise=" << Noise << "\n";
      }
      else
      {
   srand(time(NULL));
   Noise_1 = Angle(drand48() - 0.5)*0.01*tmp_1.dCast2Pi();
   tmp_1.operator+=(Noise_1);
   RobStat->SetMIPMatrixVal(3,1,tmp_1.dCast2Pi());
   cout << "KalmanFilterImu::ProcessENCMeasures()  _theta Noise=" << Noise << "\n";
      }

      // Add noise to ds measure
      tmp = RobStat->GetMIPMatrixVal(4,1);
      if(tmp == 0)
      {
   cout << "KalmanFilterImu::ProcessENCMeasures()  ds = 0" << "\n";
   srand(time(NULL));
   Noise=(drand48() - 0.5)*0.001;
   RobStat->SetMIPMatrixVal(4,1,Noise);
   cout << "KalmanFilterImu::ProcessENCMeasures()  ds Noise=" << Noise << "\n"; 
      }
      else
      {
   srand(time(NULL));
   Noise=(drand48() - 0.5)*0.01*tmp;
   tmp = tmp + Noise; 
   RobStat->SetMIPMatrixVal(4,1,tmp + Noise);
          cout << "KalmanFilterImu::ProcessENCMeasures()  ds Noise=" << Noise << "\n"; 
      }

      // Add noise to dtheta measure
      tmp_1 = RobStat->GetMIPMatrixVal(5,1);
      if(tmp_1.dCast2Pi() == 0)
      {
          cout << "KalmanFilterImu::ProcessENCMeasures()  dtheta = 0" << "\n";
   srand(time(NULL));
   Noise_1 = Angle(drand48() - 0.5)*0.01;//Circa mezzo grado di errore
   RobStat->SetMIPMatrixVal(5,1,Noise_1.dCast2Pi());
   cout << "KalmanFilterImu::ProcessENCMeasures()  dtheta Noise=" << Noise << "\n";
      }
      else
      {
   srand(time(NULL));
   Noise_1 = Angle(drand48() - 0.5)*0.01*tmp_1.dCast2Pi();
   tmp_1.operator+=(Noise_1);
   RobStat->SetMIPMatrixVal(5,1,tmp_1.dCast2Pi());
   cout << "KalmanFilterImu::ProcessENCMeasures()  dtheta Noise=" << Noise << "\n";
      }

          cout << "KalmanFilterImu::ProcessENCMeasures() with noise" << "\n"; 
      cout << "X = " << RobStat->GetMIPMatrixVal(1,1) << "\n"; 
      cout << "Y = " << RobStat->GetMIPMatrixVal(2,1) << "\n"; 
      cout << "Theta = " << RobStat->GetMIPMatrixVal(3,1) << "\n";
      cout << "ds = " << RobStat->GetMIPMatrixVal(4,1) << "\n";
      cout << "dth = " << RobStat->GetMIPMatrixVal(5,1) << "\n";
      
      meas_speed = RobStat->GetMIPMatrixVal(4,1)/ENC_DeltaTime;// The relative speed of the local reference frame w.r.t the inertial reference frame is zero
      meas_turnrate = RobStat->GetMIPMatrixVal(5,1)/ENC_DeltaTime;
 
      ENC_Meas->SetMIPMatrixVal(1,1,RobStat->GetMIPMatrixVal(1,1));
      ENC_Meas->SetMIPMatrixVal(2,1,RobStat->GetMIPMatrixVal(2,1));
      ENC_Meas->SetMIPMatrixVal(3,1,RobStat->GetMIPMatrixVal(3,1));
      ENC_Meas->SetMIPMatrixVal(4,1,meas_speed);
      ENC_Meas->SetMIPMatrixVal(5,1,meas_turnrate);
      
      cout << "KalmanFilterImu::ProcessENCMeasures() calculated ENC measures." << "\n";
      cout << "KalmanFilterImu::ProcessENCMeasures() (with noise) X      = "  << ENC_Meas->GetMIPMatrixVal(1,1) << "\n"; 
      cout << "KalmanFilterImu::ProcessENCMeasures() (with noise) Y      = "  << ENC_Meas->GetMIPMatrixVal(2,1) << "\n"; 
      cout << "KalmanFilterImu::ProcessENCMeasures() (with noise) Theta  = "  << ENC_Meas->GetMIPMatrixVal(3,1) << "\n";
      cout << "KalmanFilterImu::ProcessENCMeasures() (with noise) ds/dt  = "  << ENC_Meas->GetMIPMatrixVal(4,1) << "\n";
      cout << "KalmanFilterImu::ProcessENCMeasures() (with noise) dTh/dt = "  << ENC_Meas->GetMIPMatrixVal(5,1) << "\n";


      cout << "KalmanFilterImu::ProcessENCMeasures() end." << "\n";
      
      return;
  }


  void ProcessIMUMeasures(Acceleration3D &acc, Acceleration3D &gyro, Orientation3D &magn)
  {
      Decimal   Noise_x;
      Decimal   Noise_y;
      Decimal   Noise_z;
      
      MIPMatrix *acc_Iner;//Accelerometer measures in the Inertial Reference Frame
      MIPMatrix *gyro_Iner;//Gyro measures in the Inertial Reference Frame
      MIPMatrix *magn_Iner;//Magnetometer measures in the Inertial Reference Frame

      MIPMatrix *RotLoc2Iner;//Rotation matrix from local reference frame to inertial reference frame
      MIPMatrix *RotIner2Loc;//Rotation matrix from inertial reference frame to local reference frame
      
      cout << "KalmanFilterImu::ProcessIMUMeasures() begin." << "\n";
      cout << "KalmanFilterImu::ProcessIMUMeasures() Accelerometer measures with noise (Inertial Reference Frame)." << "\n";
      cout << "Acc x=" << acc.x() << ", Acc y=" << acc.y() << ", Acc z=" << acc.z() << "\n";
      cout << "KalmanFilterImu::ProcessIMUMeasures() Gyro measures with noise (Inertial Reference Frame)." << "\n";
      cout << "Gyro x=" << gyro.x() << ", Gyro y=" << gyro.y() << ", Gyro z=" << gyro.z() << "\n";
      cout << "KalmanFilterImu::ProcessIMUMeasures() magnetometer measures with noise (Inertial Reference Frame)." << "\n";
      cout << "Magn roll=" << magn.roll().dCast2Pi() << ", Magn pitch=" << magn.pitch().dCast2Pi() << ", Magn yaw=" << magn.yaw().dCast2Pi() << "\n";
      
      acc_Iner  = new MIPMatrix(ACC_MEAS_IMU,1,ZERO_MATRIX); //Accelerometer measures in the Inertial Reference Frame
      gyro_Iner  = new MIPMatrix(GYRO_MEAS_IMU,1,ZERO_MATRIX); //Gyro measures in the Inertial Reference Frame
      magn_Iner  = new MIPMatrix(MAGN_MEAS_IMU,1,ZERO_MATRIX); //Magnetometer measures in the Inertial Reference Frame
      acc_Iner->SetMIPMatrixVal(1,1,acc.x());
      acc_Iner->SetMIPMatrixVal(2,1,acc.y());
      acc_Iner->SetMIPMatrixVal(3,1,acc.z());
      gyro_Iner->SetMIPMatrixVal(1,1,gyro.x());
      gyro_Iner->SetMIPMatrixVal(2,1,gyro.y());
      gyro_Iner->SetMIPMatrixVal(3,1,gyro.z());
      magn_Iner->SetMIPMatrixVal(1,1,magn.roll().dCast2Pi());
      magn_Iner->SetMIPMatrixVal(2,1,magn.pitch().dCast2Pi());
      magn_Iner->SetMIPMatrixVal(3,1,magn.yaw().dCast2Pi());
      //cout << "KalmanFilterImu::ProcessIMUMeasures() Accelerometer measures with noise (Inertial Reference Frame)." << "\n";
      //cout << "Acc x=" << acc_Iner->GetMIPMatrixVal(1,1) << ", Acc y=" << acc_Iner->GetMIPMatrixVal(2,1) << ", Acc z=" << acc_Iner->GetMIPMatrixVal(3,1) << "\n";
      //cout << "KalmanFilterImu::ProcessIMUMeasures() Gyro measures with noise (Inertial Reference Frame)." << "\n";
      //cout << "Gyro x=" << gyro_Iner->GetMIPMatrixVal(1,1) << ", Gyro y=" << gyro_Iner->GetMIPMatrixVal(2,1) << ", Gyro z=" << gyro_Iner->GetMIPMatrixVal(3,1) << "\n";
      //cout << "KalmanFilterImu::ProcessIMUMeasures() magnetometer measures with noise (Inertial Reference Frame)." << "\n";
      //cout << "Magn roll=" << magn_Iner->GetMIPMatrixVal(1,1) << ", Magn pitch=" << magn_Iner->GetMIPMatrixVal(2,1) << ", Magn yaw=" << magn_Iner->GetMIPMatrixVal(3,1) << "\n";
      
      //Get rotation matrix from local to inertial reference frame
      RotLoc2Iner = new MIPMatrix(3,3,ZERO_MATRIX);
      RotIner2Loc = new MIPMatrix(3,3,ZERO_MATRIX);
         cout << "KalmanFilterImu::ProcessIMUMeasures(), RotTranslMatrix matrix: " << "\n";
      (Surf->GetRotTranslMatrix(CurrentRobotPlane))->printMIPMatrix();
      RotLoc2Iner->MinorMatrix(*(Surf->GetRotTranslMatrix(CurrentRobotPlane)),4,4);
      cout << "KalmanFilterImu::ProcessIMUMeasures(), RotLoc2Iner matrix: " << "\n";
      RotLoc2Iner->printMIPMatrix();
      
      //Calculate rotation matrix from inertial to local reference frame
      RotIner2Loc->inv33(*RotLoc2Iner);
      cout << "KalmanFilterImu::ProcessIMUMeasures(), RotIner2Loc matrix: " << "\n";
      RotIner2Loc->printMIPMatrix();

      //Translate imu measures from inertial to local reference frame
      ACC_Meas->mul(*RotIner2Loc, *acc_Iner);
      GYRO_Meas->mul(*RotIner2Loc, *gyro_Iner);
      MAGN_Meas->mul(*RotIner2Loc, *magn_Iner);
          
      //IMU measures in the local reference frame.
      cout << "KalmanFilterImu::ProcessIMUMeasures() Accelerometer measures with noise (Local Reference Frame)." << "\n";
      cout << "Acc x=" << ACC_Meas->GetMIPMatrixVal(1,1) << ", Acc y=" << ACC_Meas->GetMIPMatrixVal(2,1) << ", Acc z=" << ACC_Meas->GetMIPMatrixVal(3,1) << "\n";
      cout << "KalmanFilterImu::ProcessIMUMeasures() Gyro measures with noise (Local Reference Frame)." << "\n";
      cout << "Gyro x=" << GYRO_Meas->GetMIPMatrixVal(1,1) << ", Gyro y=" << GYRO_Meas->GetMIPMatrixVal(2,1) << ", Gyro z=" << GYRO_Meas->GetMIPMatrixVal(3,1) << "\n";
      cout << "KalmanFilterImu::ProcessIMUMeasures() magnetometer measures with noise (Local Reference Frame)." << "\n";
      cout << "Magn roll=" << MAGN_Meas->GetMIPMatrixVal(1,1) << ", Magn pitch=" << MAGN_Meas->GetMIPMatrixVal(2,1) << ", Magn yaw=" << MAGN_Meas->GetMIPMatrixVal(3,1) << "\n";
      
      delete RotLoc2Iner;
      delete RotIner2Loc;
      delete acc_Iner;
      delete gyro_Iner;
      delete magn_Iner;
      
      cout << "KalmanFilterImu::ProcessIMUMeasures() end." << "\n";
      return;
  }

 
  bool processGPSMeasures(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 << "KalmanFilterImu: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 << "KalmanFilterImu::setGPSMeasures():   Invalid GPS measures." << "\n";
   return false;
      }
      
      cout << "KalmanFilterImu::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 << "KalmanFilterImu::setGPSMeasures(): GPS data in the local reference frame:" << endl;
      GPS_Meas->printMIPMatrix();
      cout << "\n";
      cout << "KalmanFilterImu::setGPSMeasures(): GPS z_loc= " << z_loc << endl;

      return true;
  }


  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 << "KalmanFilterImu::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 << "KalmanFilterImu::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 << "KalmanFilterImu::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 << "KalmanFilterImu::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 << "KalmanFilterImu::PositionErrorEstimate(): Predicted Robot position." << "\n";
          cout << "X_pos (Predicted) = " << InerRobPos->GetMIPMatrixVal(1,1) << "\n";
      cout << "Y_pos (Predicted) = " << InerRobPos->GetMIPMatrixVal(2,1) << "\n";
      cout << "Z_pos (Predicted) = " << InerRobPos->GetMIPMatrixVal(3,1) << "\n";
      
          cout << "KalmanFilterImu::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 updateKalmanFilterImu()//Kalman filter algorithm implementation
  {
          Decimal tmp;
   Angle tmp_1;
   Decimal c_31;//Rototranslation matrix coefficient A(3,1)
   Decimal c_32;//Rototranslation matrix coefficient A(3,2)
   Decimal alpha_k;
   
          cout << "updateKalmanFilterImu() started." << endl;
   CycleTime = (ENC_DeltaTime + IMU_DeltaTime)/2;
   //Initial conditions already set at Kalman Filter instantiation. (Status prediction initialization and Pp matrix initialization). 
   //See EKF constructor. //I
   
   //cout << "Previous predicted state: " << "\n";
   //cout << "x = " << State_1->GetMIPMatrixVal(1,1) << "\n";
   //cout << "y = " << State_1->GetMIPMatrixVal(2,1) << "\n";
   //cout << "theta = " << State_1->GetMIPMatrixVal(3,1) << "\n";
   //cout << "Speed = " << State_1->GetMIPMatrixVal(4,1) << "\n";
   //cout << "Ang rate = " << State_1->GetMIPMatrixVal(5,1) << "\n";
 
   //cout << "Previous estimated state: " << "\n";
   //cout << "x = " << State_0->GetMIPMatrixVal(1,1) << "\n";
   //cout << "y = " << State_0->GetMIPMatrixVal(2,1) << "\n";
   //cout << "theta = " << State_0->GetMIPMatrixVal(3,1) << "\n";
   //cout << "Speed = " << State_0->GetMIPMatrixVal(4,1) << "\n";
   //cout << "Ang rate = " << State_0->GetMIPMatrixVal(5,1) << "\n";
   
//    if(GPS_MeasAvail == true)
//    {
//    cout << "updateKalmanFilterImu() - 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).
        
   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
       
      cout << "updateKalmanFilterImu() - GPS measure not available." << endl;
      //Calculates alfa(k) coefficient. See tesis pag 67.
      c_31 = Surf->GetRotTranslMatrixElem(CurrentRobotPlane, 3, 1);
      c_32 = Surf->GetRotTranslMatrixElem(CurrentRobotPlane, 3, 2);
       
      cout << "KalmanFilterImu::updateKalmanFilterImu(), CurrentRobotPlane: " << CurrentRobotPlane 
           << "; Rototranslation matrix coefficient A(3,1): "
    << c_31 << "; Rototranslation matrix coefficient A(3,2): " << c_32 << endl;
       
      //The only component of the linear acceleration of the robot is the one directed along the positive Local Reference Frame X axis. 
      //The other two components are compensated by the friction force. So the component to be considered is: ACC_Meas->GetMIPMatrixVal(1,1).
      //alpha_k = GRAV*(c_31*cos(State_0->GetMIPMatrixVal(3,1)) + c_32*sin(State_0->GetMIPMatrixVal(3,1)) + Force/RobotMass);//To put IMU accel measure
      alpha_k = (c_31*cos(State_0->GetMIPMatrixVal(3,1)) + c_32*sin(State_0->GetMIPMatrixVal(3,1))*GRAV + ACC_Meas->GetMIPMatrixVal(1,1));

      //x1 = x1_0 + T*v*cos(th)
      State_1->SetMIPMatrixVal(1, 1, State_1->GetMIPMatrixVal(1,1) + CycleTime*State_1->GetMIPMatrixVal(4,1)*cos(State_1->GetMIPMatrixVal(3,1)));

      //y1 = y1_0 + T*v*sin(th)
      State_1->SetMIPMatrixVal(2, 1, State_1->GetMIPMatrixVal(2,1) + CycleTime*State_1->GetMIPMatrixVal(4,1)*sin(State_1->GetMIPMatrixVal(3,1)));

      //th1 = th1_0 + T*w    
      State_1->SetMIPMatrixVal(3, 1, Angle(State_1->GetMIPMatrixVal(3,1) + CycleTime*State_1->GetMIPMatrixVal(5,1)).dCast());

      //v1 = v1_0 + T*alpha_k     
      State_1->SetMIPMatrixVal(4, 1, State_1->GetMIPMatrixVal(4,1) + CycleTime*alpha_k);

      //w1 = w1_0 + T*Torque/RobotMoI
      //The only component of the angular acceleration of the robot is the one directed along the positive Local Reference Frame Z axis. 
      //The other two components are compensated by the friction force of the plane.So the component to be considered is: GYRO_Meas->GetMIPMatrixVal(3,1).
      //State_1->SetMIPMatrixVal(5, 1, State_1->GetMIPMatrixVal(5,1) + CycleTime*Torque/RobotMoI);//To put IMU gyro measure  
      State_1->SetMIPMatrixVal(5, 1, State_1->GetMIPMatrixVal(5,1) + CycleTime*GYRO_Meas->GetMIPMatrixVal(3,1));
   }  */ 
   
   cout << "Predicted state: " << "\n";
   cout << "x = " << State_1->GetMIPMatrixVal(1,1) << "\n";
   cout << "y = " << State_1->GetMIPMatrixVal(2,1) << "\n";
   cout << "theta = " << State_1->GetMIPMatrixVal(3,1) << "\n";
   cout << "Speed = " << State_1->GetMIPMatrixVal(4,1) << "\n";
   cout << "Ang rate = " << State_1->GetMIPMatrixVal(5,1) << "\n";
   
   cout << "Estimated state: " << "\n";
   cout << "x = " << State_0->GetMIPMatrixVal(1,1) << "\n";
   cout << "y = " << State_0->GetMIPMatrixVal(2,1) << "\n";
   cout << "theta = " << State_0->GetMIPMatrixVal(3,1) << "\n";
   cout << "Speed = " << State_0->GetMIPMatrixVal(4,1) << "\n";
   cout << "Ang rate = " << State_0->GetMIPMatrixVal(5,1) << "\n";
   
   return;
  }
};
#endif