Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

implementation problem of my EKF program in C++

Hello, I implemented EKF in c++;But the output value (pitch, yaw ,roll ) is not correct . Please giv me some advice if you find my code's problem.

I used MPU9150 and I implemented EKF by referring to the following URL;

https://github.com/gakuseishitsu/ExtendedKalmanFilterForIMU

the following values are output of EKF while MPU9150 is not moved and placed on the flat table .

data.pitch_ekf:89.7621 data.yaw_ekf: 85.745 data.roll:-0.719216

data.pitch_sens:-1.59499 data.yaw_sens:7.9985 data.roll_sens:0

[my source code]

#include <iostream>
#include <string>
#include <vector>
#include <fstream> // ifstream, ofstream
#include <sstream> // istringstream
#include <cmath>
#include <eigen3/Eigen/Core> 
#include <eigen3/Eigen/LU>  
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include "my_simple_ekf/send_msg.h"
const int NUM_OF_IMU_DATA = 6;
const double FIRST_TIME = 0.01;
const double DT = 0.01;
const double GRAV_ACC = 9.80665;
const double TO_DEG = 57.2958; // rad -> deg
const double IMU_LPF = 0.07;
const std::vector<double> coordinateModifyFactor{1.0, -1.0, 1.0, -1.0, 1.0, -1.0}; 
const std::vector<double> BIAS{-0.037, 0.0, 0.0, -0.148, 0.046, 0.189}; 

const std::vector<double> INIT_P{ 0.0001, 0.0001, 0.0001, 0.0001 };
const std::vector<double> INIT_Q{ 0.01, 0.01, 0.01, 0.01 };
const std::vector<double> INIT_R{ 1.0, 1.0, 10.0 };
const std::vector<double> INIT_X { 0.0, 0.0, 0.0, 1.0 };
const int SIZE_VECTOR_X = 4;
const int SIZE_VECTOR_Y = 3;

template<typename T>
class u {
private:
T t;

ros::NodeHandle nh;
    ros::Subscriber ros_imu_sub; 
    ros::Subscriber ros_mag_sub;


public:
u(const int num);
~u() {};
    std::vector<T> imuRawData;
    std::vector<T> imuDataLPF;
    std::vector<T> imuDataBias;
void setImuDataBias(const std::vector<T>& b);
void setLPF(const T ft, const T lpf);
    void setUData(const std::vector<T>& f);
    ros::Publisher  twist_pub;
    ros::Publisher  data_pub;
T getT() const { return t; };
    void imuCallback(const sensor_msgs::Imu& imu_msg);
    void magCallback(const sensor_msgs::MagneticField& msg);
};

// constructor of u
template<typename T>
u<T>::u(const int num)
{
   twist_pub = nh.advertise<geometry_msgs::Twist>("angle_simple_ekf",10);

   data_pub = nh.advertise<my_simple_ekf::send_msg>("save_data",10);

   ros_imu_sub = nh.subscribe("imu",100, &u<T>::imuCallback,this);

   ros_mag_sub = nh.subscribe("mag",100, &u<T>::magCallback,this);

   for (int i = 0; i < num; ++i) {
    imuRawData.push_back(static_cast<T>(0));
imuDataBias.push_back(static_cast<T>(0));
imuDataLPF.push_back(static_cast<T>(0));
    }
 }

template<typename T>
void u<T>::setImuDataBias(const std::vector<T>& b) {

for (int i = 0; i < static_cast<int>(imuRawData.size()); i++)
    imuDataBias[i] = imuRawData[i] - b[i];

}

template<typename T>
void u<T>::setLPF(const T ft, const T lpf) {
  for (int i = 0; i < static_cast<int>(imuRawData.size()); i++)
     imuDataLPF[i] = /*(lpf)**/imuDataBias[i];//+ (1.0 - lpf) * imuDataLPF[i];
}

template<typename T>
void u<T>::setUData(const std::vector<T>& f) {
    for (int i = 0; i < static_cast<int>(imuRawData.size()); i++)
    imuRawData[i] = f[i] * imuRawData[i];
}


template<typename T>
void u<T>::imuCallback(const sensor_msgs::Imu& imu_msg)
{

  imuRawData[0] = imu_msg.linear_acceleration.x;
  imuRawData[1] = imu_msg.linear_acceleration.y;
  imuRawData[2] = imu_msg.linear_acceleration.z;
  imuRawData[3] = imu_msg.angular_velocity.x;
  imuRawData[4] = imu_msg.angular_velocity.y;
  imuRawData[5] = imu_msg.angular_velocity.z;
}

template<typename T>
std::ostream& operator<<(std::ostream& os, const u<T>& udata);

template<typename T>
class myKalmanFilter {
private:

Eigen::Matrix<T, SIZE_VECTOR_X, 1> xHatMinus;       // Advance estimate of the state valiable
Eigen::Matrix<T, SIZE_VECTOR_Y, 1> y;           // Observation value
Eigen::Matrix<T, SIZE_VECTOR_X, 1> f;           // System Matrix
Eigen::Matrix<T, SIZE_VECTOR_Y, 1> h;           // Transformation matrix from state valiable to observation value
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> A;   // Linearized f
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> AT;  // Transposition of A
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_Y> C;   // Liniarized h
Eigen::Matrix<T, SIZE_VECTOR_Y, SIZE_VECTOR_X> CT;  // Transposition of C
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_Y> g;   // Kalman gain
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> P;   // Covariance matrix
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> PMinus;  // Advance covariance matrix
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> Q;   // Variance matrix of w
Eigen::Matrix<T, SIZE_VECTOR_Y, SIZE_VECTOR_Y> R;   // Variance matrix of v
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> I;   // Unit Matrix
public:
myKalmanFilter(const std::vector<T>& x, const std::vector<T>& p, const std::vector<T>& q, const std::vector<T>& r);
~myKalmanFilter() {};
    Eigen::Matrix<T, SIZE_VECTOR_X, 1> xHat;        // Estimated value of the state valiable

void prediction(std::vector<T>& imu);
void filter(std::vector<T>& imu);

void normXHat();
};


// constructor of myKalmanFilter
template<typename T>
myKalmanFilter<T>::myKalmanFilter(const std::vector<T>& x, const std::vector<T>& p, const std::vector<T>& q, const             
std::vector<T>& r)
{
xHat << x[0], 
        x[1], 
        x[2], 
        x[3];

P << p[0]  , 0.0, 0.0, 0.0,
     0.0, p[1]  , 0.0, 0.0,
     0.0, 0.0, p[2]  , 0.0,
     0.0, 0.0, 0.0  , p[3];

Q << q[0]  , 0.0, 0.0, 0.0,
     0.0, q[1]  , 0.0, 0.0,
     0.0, 0.0, q[2]  , 0.0,
     0.0, 0.0, 0.0, q[3]  ;

R << r[0]  , 0.0, 0.0,
     0.0, r[1]  , 0.0,
     0.0, 0.0, r[2]  ;

I << 1.0, 0.0, 0.0, 0.0,
     0.0, 1.0, 0.0, 0.0,
     0.0, 0.0, 1.0, 0.0,
     0.0, 0.0, 0.0, 1.0;

// fill other matrix 0
xHatMinus.setZero();
y.setZero();
f.setZero();
h.setZero();
PMinus.setZero();
g.setZero();
A.setZero();
AT.setZero();
C.setZero();
CT.setZero();
}


template<typename T>
void myKalmanFilter<T>::prediction(std::vector<T>& imu)
{

std::vector<T> w,q;
w.push_back(static_cast<T>(0));
for (int i = 3; i < 6; ++i)
    w.push_back(imu[i]); // w[1]~w[3]

A <<      1.0    ,  0.5*DT*w[3], -0.5*DT*w[2], 0.5*DT*w[1],
     -0.5*DT*w[3],      1.0    ,  0.5*DT*w[1], 0.5*DT*w[2],
      0.5*DT*w[2], -0.5*DT*w[1],      1.0    , 0.5*DT*w[3],
     -0.5*DT*w[1], -0.5*DT*w[2], -0.5*DT*w[3],     1.0    ;

    std::cout << "A:" << A << std::endl;

f = A * xHat;
xHatMinus = f;

q.push_back(static_cast<T>(0));
for (int i = 0; i < 4; ++i)
    q.push_back(xHatMinus[i]); // q[1]~q[4]

CT <<  2 * q[3] * GRAV_ACC, -2 * q[4] * GRAV_ACC, 2 * q[1] * GRAV_ACC, -2 * q[2] * GRAV_ACC,
       2 * q[4] * GRAV_ACC,  2 * q[3] * GRAV_ACC, 2 * q[2] * GRAV_ACC,  2 * q[1] * GRAV_ACC,
      -2 * q[1] * GRAV_ACC, -2 * q[2] * GRAV_ACC, 2 * q[3] * GRAV_ACC,  2 * q[4] * GRAV_ACC;

    std::cout << "CT:" << CT << std::endl;

PMinus = A * P * A.transpose() + Q;

}

template<typename T>
void myKalmanFilter<T>::filter(std::vector<T>& imu) {
std::vector<T> a, q;
a.push_back(static_cast<T>(0));
for (int i = 0; i < 3; ++i)
    a.push_back(imu[i]); // a[1]~a[3]
q.push_back(static_cast<T>(0));
for (int i = 0; i < 4; ++i)
    q.push_back(xHatMinus[i]); // q[1]~q[4]

C = CT.transpose();

g = PMinus * C * (CT * PMinus * C + R).inverse();

y << a[1],
     a[2],
     a[3];

h << 2 * (q[3] * q[1] - q[2] * q[4]) * GRAV_ACC,
     2 * (q[2] * q[3] - q[1] * q[4]) * GRAV_ACC,
     (q[3] * q[3] - q[1] * q[1] - q[2] * q[2] + q[4] * q[4]) * GRAV_ACC;

xHat = xHatMinus + g * (y - h);
//xHat = xHatMinus;

P = ( I - g * CT ) * PMinus;
}

template<typename T>
void myKalmanFilter<T>::normXHat() {
T norm;
norm = sqrt(xHat(0, 0)*xHat(0, 0) + xHat(1, 0)*xHat(1, 0) + xHat(2, 0)*xHat(2, 0) + xHat(3, 0)*xHat(3, 0));
xHat /= norm;
}



template<typename T>
Eigen::Matrix<T, 3, 3> getDCM(Eigen::Matrix<T, SIZE_VECTOR_X, 1>& q);

template<typename T>
Eigen::Matrix<T, 3, 1> getEuler(Eigen::Matrix<T, 3, 3>& dcm);

template<typename T>
Eigen::Matrix<T, 3, 1> getEulerFromAccData(std::vector<T>& imu);

int main(int argc, char** argv)
{
ros::init(argc,argv,"my_simple_ekf");

u<double> input(NUM_OF_IMU_DATA);
myKalmanFilter<double> kalman(INIT_X, INIT_P, INIT_Q, INIT_R);

// looped in 100Hz
ros::Rate loop_rate(10);

 geometry_msgs::Twist twist;
 my_simple_ekf::send_msg data;
while (ros::ok())
{
            ros::spinOnce(); 
    // string data -> vector, bias filter, LPF
            input.setUData(coordinateModifyFactor);
        input.setImuDataBias(BIAS);

    input.setLPF(FIRST_TIME, IMU_LPF);
    //  std::cout << "LFPdata: " << input << std::endl << std::/*
    // kalman filter and normalize quaternion
    kalman.prediction(input.imuDataLPF);//input.getImuDataLPF());
    kalman.filter(input.imuDataLPF);
    kalman.normXHat();
    //std::cout << input.getT() << "," << kalman << std::endl;
    //out << input.getT() << "," << kalman << std::endl;

    // get DCM and Euler angles from quaternion
    Eigen::Matrix<double, 3, 3> DCM = getDCM(kalman.xHat);
    Eigen::Matrix<double, 3, 1> E = getEuler(DCM);

            Eigen::Matrix<double, 3, 1> e = getEulerFromAccData(input.imuDataLPF);//input.getImuDataLPF());

            twist.angular.x = E(0,0);     // pitch
            twist.angular.y = E(1,0);     // yaw
            twist.angular.z = E(2,0);     // roll   

            input.twist_pub.publish(twist);

            data.accelX = input.imuRawData[0];  // accel ax
            data.accelY = input.imuRawData[1];  // accel ay
            data.accelZ = input.imuRawData[2];  // accel az
            data.gyroX  = input.imuRawData[3];  // angular velocity ωx
            data.gyroY  = input.imuRawData[4];  // angular velocity ωy
            data.gyroZ  = input.imuRawData[5];  // angular velocity ωz
            data.magX   = input.imuRawData[6];  // magnatic mx
            data.magY   = input.imuRawData[7];  // magnatic my
            data.magZ   = input.imuRawData[8];  // magnatic mz
            data.pitch  = E(0,0)*TO_DEG;         // pitch
            data.yaw    = E(1,0)*TO_DEG;         // yaw
            data.roll   = E(2,0)*TO_DEG;         // roll
            input.data_pub.publish(data);


    std::cout << "data.pitch_ekf: " << data.pitch << std::endl ;
    std::cout << "data.yaw_ekf: " << data.yaw << std::endl ;
    std::cout << "data.roll_ekf: " << data.roll << std::endl ;
    std::cout << "data.pitch_sens: " << e(0,0)*TO_DEG << std::endl ;
    std::cout << "data.yaw_sens: " << e(1,0)*TO_DEG << std::endl ;
    std::cout << "data.roll_sens: " << e(2,0)*TO_DEG << std::endl ;

            loop_rate.sleep();
}

return 0;
}

template<typename T>
Eigen::Matrix<T, 3, 3> getDCM(Eigen::Matrix<T, SIZE_VECTOR_X, 1>& x) {
Eigen::Matrix<T, 3, 3> dcm;
std::vector<T> q;
q.push_back(static_cast<T>(0));
for (int i = 0; i < 4; ++i)
    q.push_back(x(i,0)); // q[1]~q[4]

dcm << q[1] * q[1] - q[2] * q[2] - q[3] * q[3] + q[4] * q[4], 2 * (q[1] * q[2] + q[3] * q[4]), 2 * (q[3] * q[1] - q[2] * q[4]),
    2 * (q[1] * q[2] - q[3] * q[4]), q[2] * q[2] - q[3] * q[3] - q[1] * q[1] + q[4] * q[4], 2 * (q[2] * q[3] + q[1] * q[4]),
    2 * (q[3] * q[1] + q[2] * q[4]), 2 * (q[2] * q[3] - q[1] * q[4]), q[3] * q[3] - q[1] * q[1] - q[2] * q[2] + q[4] * q[4];
std::cout << "dcm:" << dcm <<std::endl;
return dcm;
}

template<typename T>
Eigen::Matrix<T, 3, 1> getEuler(Eigen::Matrix<T, 3, 3>& dcm) {
Eigen::Matrix<T, 3, 1> euler;
T pitch, yaw, roll;

    pitch = atan(dcm(1,2)/dcm(2,2));
yaw = atan(-dcm(0,2)/sqrt(dcm(1,2)*dcm(1,2)+dcm(2,2)*dcm(2,2)));
roll =atan(dcm(0,1)/dcm(0,0));
/*
pitch=asin(-1.0 * dcm(0, 2));
yaw = atan2(dcm(0, 1) / cos(pitch), dcm(0, 0) / cos(pitch));
roll = atan2(dcm(1, 2) / cos(pitch), dcm(2, 2) / cos(pitch));
*/
euler << roll,
    pitch,
    yaw;

return euler;
}


template<typename T>
Eigen::Matrix<T, 3, 1> getEulerFromAccData(std::vector<T>& imu) {
Eigen::Matrix<T, 3, 1> euler;


std::cout << "imu[0]: " << imu[0] << std::endl ;
std::cout << "imu[1]: " << imu[1] << std::endl ;
std::cout << "imu[2]:" << imu[2] << std::endl ;


T roll, pitch, yaw, acc;

acc = sqrt(imu[0] * imu[0] + imu[1] * imu[1] + imu[2] * imu[2]);
/*
pitch = asin(-1.0 * imu[0] / acc);
yaw = static_cast<T>(0);
if(imu[2] > 0.0)
    roll = atan2(imu[1] / acc / cos(pitch), imu[2] / acc / cos(pitch));
else
    roll = atan2(imu[1] / acc / cos(pitch), imu[2] / acc / cos(pitch));
*/
imu[0] = imu[0]/acc;
imu[1] = imu[1]/acc;
imu[2] = imu[2]/acc;
yaw = static_cast<T>(0);
pitch = atan(imu[0]/sqrt(imu[1]*imu[1]+imu[2]*imu[2]));
roll = atan(imu[1]/imu[2]);
euler << roll,
    pitch,
    yaw;

return euler;
}

implementation problem of my EKF program in C++

Hello, I implemented EKF in c++;But the output value (pitch, yaw ,roll ) is not correct . Please giv give me some advice if you find my code's problem.

I used MPU9150 and I implemented EKF by referring to the following URL;

https://github.com/gakuseishitsu/ExtendedKalmanFilterForIMU

the following values are output of EKF while MPU9150 is not moved and placed on the flat table .

data.pitch_ekf:89.7621 data.yaw_ekf: 85.745 data.roll:-0.719216data.roll_ekf:-0.719216

data.pitch_sens:-1.59499 data.yaw_sens:7.9985 data.roll_sens:0

[my source code]

#include <iostream>
#include <string>
#include <vector>
#include <fstream> // ifstream, ofstream
#include <sstream> // istringstream
#include <cmath>
#include <eigen3/Eigen/Core> 
#include <eigen3/Eigen/LU>  
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include "my_simple_ekf/send_msg.h"
const int NUM_OF_IMU_DATA = 6;
const double FIRST_TIME = 0.01;
const double DT = 0.01;
const double GRAV_ACC = 9.80665;
const double TO_DEG = 57.2958; // rad -> deg
const double IMU_LPF = 0.07;
const std::vector<double> coordinateModifyFactor{1.0, -1.0, 1.0, -1.0, 1.0, -1.0}; 
const std::vector<double> BIAS{-0.037, 0.0, 0.0, -0.148, 0.046, 0.189}; 

const std::vector<double> INIT_P{ 0.0001, 0.0001, 0.0001, 0.0001 };
const std::vector<double> INIT_Q{ 0.01, 0.01, 0.01, 0.01 };
const std::vector<double> INIT_R{ 1.0, 1.0, 10.0 };
const std::vector<double> INIT_X { 0.0, 0.0, 0.0, 1.0 };
const int SIZE_VECTOR_X = 4;
const int SIZE_VECTOR_Y = 3;

template<typename T>
class u {
private:
T t;

ros::NodeHandle nh;
    ros::Subscriber ros_imu_sub; 
    ros::Subscriber ros_mag_sub;


public:
u(const int num);
~u() {};
    std::vector<T> imuRawData;
    std::vector<T> imuDataLPF;
    std::vector<T> imuDataBias;
void setImuDataBias(const std::vector<T>& b);
void setLPF(const T ft, const T lpf);
    void setUData(const std::vector<T>& f);
    ros::Publisher  twist_pub;
    ros::Publisher  data_pub;
T getT() const { return t; };
    void imuCallback(const sensor_msgs::Imu& imu_msg);
    void magCallback(const sensor_msgs::MagneticField& msg);
};

// constructor of u
template<typename T>
u<T>::u(const int num)
{
   twist_pub = nh.advertise<geometry_msgs::Twist>("angle_simple_ekf",10);

   data_pub = nh.advertise<my_simple_ekf::send_msg>("save_data",10);

   ros_imu_sub = nh.subscribe("imu",100, &u<T>::imuCallback,this);

   ros_mag_sub = nh.subscribe("mag",100, &u<T>::magCallback,this);

   for (int i = 0; i < num; ++i) {
    imuRawData.push_back(static_cast<T>(0));
imuDataBias.push_back(static_cast<T>(0));
imuDataLPF.push_back(static_cast<T>(0));
    }
 }

template<typename T>
void u<T>::setImuDataBias(const std::vector<T>& b) {

for (int i = 0; i < static_cast<int>(imuRawData.size()); i++)
    imuDataBias[i] = imuRawData[i] - b[i];

}

template<typename T>
void u<T>::setLPF(const T ft, const T lpf) {
  for (int i = 0; i < static_cast<int>(imuRawData.size()); i++)
     imuDataLPF[i] = /*(lpf)**/imuDataBias[i];//+ (1.0 - lpf) * imuDataLPF[i];
}

template<typename T>
void u<T>::setUData(const std::vector<T>& f) {
    for (int i = 0; i < static_cast<int>(imuRawData.size()); i++)
    imuRawData[i] = f[i] * imuRawData[i];
}


template<typename T>
void u<T>::imuCallback(const sensor_msgs::Imu& imu_msg)
{

  imuRawData[0] = imu_msg.linear_acceleration.x;
  imuRawData[1] = imu_msg.linear_acceleration.y;
  imuRawData[2] = imu_msg.linear_acceleration.z;
  imuRawData[3] = imu_msg.angular_velocity.x;
  imuRawData[4] = imu_msg.angular_velocity.y;
  imuRawData[5] = imu_msg.angular_velocity.z;
}

template<typename T>
std::ostream& operator<<(std::ostream& os, const u<T>& udata);

template<typename T>
class myKalmanFilter {
private:

Eigen::Matrix<T, SIZE_VECTOR_X, 1> xHatMinus;       // Advance estimate of the state valiable
Eigen::Matrix<T, SIZE_VECTOR_Y, 1> y;           // Observation value
Eigen::Matrix<T, SIZE_VECTOR_X, 1> f;           // System Matrix
Eigen::Matrix<T, SIZE_VECTOR_Y, 1> h;           // Transformation matrix from state valiable to observation value
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> A;   // Linearized f
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> AT;  // Transposition of A
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_Y> C;   // Liniarized h
Eigen::Matrix<T, SIZE_VECTOR_Y, SIZE_VECTOR_X> CT;  // Transposition of C
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_Y> g;   // Kalman gain
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> P;   // Covariance matrix
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> PMinus;  // Advance covariance matrix
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> Q;   // Variance matrix of w
Eigen::Matrix<T, SIZE_VECTOR_Y, SIZE_VECTOR_Y> R;   // Variance matrix of v
Eigen::Matrix<T, SIZE_VECTOR_X, SIZE_VECTOR_X> I;   // Unit Matrix
public:
myKalmanFilter(const std::vector<T>& x, const std::vector<T>& p, const std::vector<T>& q, const std::vector<T>& r);
~myKalmanFilter() {};
    Eigen::Matrix<T, SIZE_VECTOR_X, 1> xHat;        // Estimated value of the state valiable

void prediction(std::vector<T>& imu);
void filter(std::vector<T>& imu);

void normXHat();
};


// constructor of myKalmanFilter
template<typename T>
myKalmanFilter<T>::myKalmanFilter(const std::vector<T>& x, const std::vector<T>& p, const std::vector<T>& q, const             
std::vector<T>& r)
{
xHat << x[0], 
        x[1], 
        x[2], 
        x[3];

P << p[0]  , 0.0, 0.0, 0.0,
     0.0, p[1]  , 0.0, 0.0,
     0.0, 0.0, p[2]  , 0.0,
     0.0, 0.0, 0.0  , p[3];

Q << q[0]  , 0.0, 0.0, 0.0,
     0.0, q[1]  , 0.0, 0.0,
     0.0, 0.0, q[2]  , 0.0,
     0.0, 0.0, 0.0, q[3]  ;

R << r[0]  , 0.0, 0.0,
     0.0, r[1]  , 0.0,
     0.0, 0.0, r[2]  ;

I << 1.0, 0.0, 0.0, 0.0,
     0.0, 1.0, 0.0, 0.0,
     0.0, 0.0, 1.0, 0.0,
     0.0, 0.0, 0.0, 1.0;

// fill other matrix 0
xHatMinus.setZero();
y.setZero();
f.setZero();
h.setZero();
PMinus.setZero();
g.setZero();
A.setZero();
AT.setZero();
C.setZero();
CT.setZero();
}


template<typename T>
void myKalmanFilter<T>::prediction(std::vector<T>& imu)
{

std::vector<T> w,q;
w.push_back(static_cast<T>(0));
for (int i = 3; i < 6; ++i)
    w.push_back(imu[i]); // w[1]~w[3]

A <<      1.0    ,  0.5*DT*w[3], -0.5*DT*w[2], 0.5*DT*w[1],
     -0.5*DT*w[3],      1.0    ,  0.5*DT*w[1], 0.5*DT*w[2],
      0.5*DT*w[2], -0.5*DT*w[1],      1.0    , 0.5*DT*w[3],
     -0.5*DT*w[1], -0.5*DT*w[2], -0.5*DT*w[3],     1.0    ;

    std::cout << "A:" << A << std::endl;

f = A * xHat;
xHatMinus = f;

q.push_back(static_cast<T>(0));
for (int i = 0; i < 4; ++i)
    q.push_back(xHatMinus[i]); // q[1]~q[4]

CT <<  2 * q[3] * GRAV_ACC, -2 * q[4] * GRAV_ACC, 2 * q[1] * GRAV_ACC, -2 * q[2] * GRAV_ACC,
       2 * q[4] * GRAV_ACC,  2 * q[3] * GRAV_ACC, 2 * q[2] * GRAV_ACC,  2 * q[1] * GRAV_ACC,
      -2 * q[1] * GRAV_ACC, -2 * q[2] * GRAV_ACC, 2 * q[3] * GRAV_ACC,  2 * q[4] * GRAV_ACC;

    std::cout << "CT:" << CT << std::endl;

PMinus = A * P * A.transpose() + Q;

}

template<typename T>
void myKalmanFilter<T>::filter(std::vector<T>& imu) {
std::vector<T> a, q;
a.push_back(static_cast<T>(0));
for (int i = 0; i < 3; ++i)
    a.push_back(imu[i]); // a[1]~a[3]
q.push_back(static_cast<T>(0));
for (int i = 0; i < 4; ++i)
    q.push_back(xHatMinus[i]); // q[1]~q[4]

C = CT.transpose();

g = PMinus * C * (CT * PMinus * C + R).inverse();

y << a[1],
     a[2],
     a[3];

h << 2 * (q[3] * q[1] - q[2] * q[4]) * GRAV_ACC,
     2 * (q[2] * q[3] - q[1] * q[4]) * GRAV_ACC,
     (q[3] * q[3] - q[1] * q[1] - q[2] * q[2] + q[4] * q[4]) * GRAV_ACC;

xHat = xHatMinus + g * (y - h);
//xHat = xHatMinus;

P = ( I - g * CT ) * PMinus;
}

template<typename T>
void myKalmanFilter<T>::normXHat() {
T norm;
norm = sqrt(xHat(0, 0)*xHat(0, 0) + xHat(1, 0)*xHat(1, 0) + xHat(2, 0)*xHat(2, 0) + xHat(3, 0)*xHat(3, 0));
xHat /= norm;
}



template<typename T>
Eigen::Matrix<T, 3, 3> getDCM(Eigen::Matrix<T, SIZE_VECTOR_X, 1>& q);

template<typename T>
Eigen::Matrix<T, 3, 1> getEuler(Eigen::Matrix<T, 3, 3>& dcm);

template<typename T>
Eigen::Matrix<T, 3, 1> getEulerFromAccData(std::vector<T>& imu);

int main(int argc, char** argv)
{
ros::init(argc,argv,"my_simple_ekf");

u<double> input(NUM_OF_IMU_DATA);
myKalmanFilter<double> kalman(INIT_X, INIT_P, INIT_Q, INIT_R);

// looped in 100Hz
ros::Rate loop_rate(10);

 geometry_msgs::Twist twist;
 my_simple_ekf::send_msg data;
while (ros::ok())
{
            ros::spinOnce(); 
    // string data -> vector, bias filter, LPF
            input.setUData(coordinateModifyFactor);
        input.setImuDataBias(BIAS);

    input.setLPF(FIRST_TIME, IMU_LPF);
    //  std::cout << "LFPdata: " << input << std::endl << std::/*
    // kalman filter and normalize quaternion
    kalman.prediction(input.imuDataLPF);//input.getImuDataLPF());
    kalman.filter(input.imuDataLPF);
    kalman.normXHat();
    //std::cout << input.getT() << "," << kalman << std::endl;
    //out << input.getT() << "," << kalman << std::endl;

    // get DCM and Euler angles from quaternion
    Eigen::Matrix<double, 3, 3> DCM = getDCM(kalman.xHat);
    Eigen::Matrix<double, 3, 1> E = getEuler(DCM);

            Eigen::Matrix<double, 3, 1> e = getEulerFromAccData(input.imuDataLPF);//input.getImuDataLPF());

            twist.angular.x = E(0,0);     // pitch
            twist.angular.y = E(1,0);     // yaw
            twist.angular.z = E(2,0);     // roll   

            input.twist_pub.publish(twist);

            data.accelX = input.imuRawData[0];  // accel ax
            data.accelY = input.imuRawData[1];  // accel ay
            data.accelZ = input.imuRawData[2];  // accel az
            data.gyroX  = input.imuRawData[3];  // angular velocity ωx
            data.gyroY  = input.imuRawData[4];  // angular velocity ωy
            data.gyroZ  = input.imuRawData[5];  // angular velocity ωz
            data.magX   = input.imuRawData[6];  // magnatic mx
            data.magY   = input.imuRawData[7];  // magnatic my
            data.magZ   = input.imuRawData[8];  // magnatic mz
            data.pitch  = E(0,0)*TO_DEG;         // pitch
            data.yaw    = E(1,0)*TO_DEG;         // yaw
            data.roll   = E(2,0)*TO_DEG;         // roll
            input.data_pub.publish(data);


    std::cout << "data.pitch_ekf: " << data.pitch << std::endl ;
    std::cout << "data.yaw_ekf: " << data.yaw << std::endl ;
    std::cout << "data.roll_ekf: " << data.roll << std::endl ;
    std::cout << "data.pitch_sens: " << e(0,0)*TO_DEG << std::endl ;
    std::cout << "data.yaw_sens: " << e(1,0)*TO_DEG << std::endl ;
    std::cout << "data.roll_sens: " << e(2,0)*TO_DEG << std::endl ;

            loop_rate.sleep();
}

return 0;
}

template<typename T>
Eigen::Matrix<T, 3, 3> getDCM(Eigen::Matrix<T, SIZE_VECTOR_X, 1>& x) {
Eigen::Matrix<T, 3, 3> dcm;
std::vector<T> q;
q.push_back(static_cast<T>(0));
for (int i = 0; i < 4; ++i)
    q.push_back(x(i,0)); // q[1]~q[4]

dcm << q[1] * q[1] - q[2] * q[2] - q[3] * q[3] + q[4] * q[4], 2 * (q[1] * q[2] + q[3] * q[4]), 2 * (q[3] * q[1] - q[2] * q[4]),
    2 * (q[1] * q[2] - q[3] * q[4]), q[2] * q[2] - q[3] * q[3] - q[1] * q[1] + q[4] * q[4], 2 * (q[2] * q[3] + q[1] * q[4]),
    2 * (q[3] * q[1] + q[2] * q[4]), 2 * (q[2] * q[3] - q[1] * q[4]), q[3] * q[3] - q[1] * q[1] - q[2] * q[2] + q[4] * q[4];
std::cout << "dcm:" << dcm <<std::endl;
return dcm;
}

template<typename T>
Eigen::Matrix<T, 3, 1> getEuler(Eigen::Matrix<T, 3, 3>& dcm) {
Eigen::Matrix<T, 3, 1> euler;
T pitch, yaw, roll;

    pitch = atan(dcm(1,2)/dcm(2,2));
yaw = atan(-dcm(0,2)/sqrt(dcm(1,2)*dcm(1,2)+dcm(2,2)*dcm(2,2)));
roll =atan(dcm(0,1)/dcm(0,0));
/*
pitch=asin(-1.0 * dcm(0, 2));
yaw = atan2(dcm(0, 1) / cos(pitch), dcm(0, 0) / cos(pitch));
roll = atan2(dcm(1, 2) / cos(pitch), dcm(2, 2) / cos(pitch));
*/
euler << roll,
    pitch,
    yaw;

return euler;
}


template<typename T>
Eigen::Matrix<T, 3, 1> getEulerFromAccData(std::vector<T>& imu) {
Eigen::Matrix<T, 3, 1> euler;


std::cout << "imu[0]: " << imu[0] << std::endl ;
std::cout << "imu[1]: " << imu[1] << std::endl ;
std::cout << "imu[2]:" << imu[2] << std::endl ;


T roll, pitch, yaw, acc;

acc = sqrt(imu[0] * imu[0] + imu[1] * imu[1] + imu[2] * imu[2]);
/*
pitch = asin(-1.0 * imu[0] / acc);
yaw = static_cast<T>(0);
if(imu[2] > 0.0)
    roll = atan2(imu[1] / acc / cos(pitch), imu[2] / acc / cos(pitch));
else
    roll = atan2(imu[1] / acc / cos(pitch), imu[2] / acc / cos(pitch));
*/
imu[0] = imu[0]/acc;
imu[1] = imu[1]/acc;
imu[2] = imu[2]/acc;
yaw = static_cast<T>(0);
pitch = atan(imu[0]/sqrt(imu[1]*imu[1]+imu[2]*imu[2]));
roll = atan(imu[1]/imu[2]);
euler << roll,
    pitch,
    yaw;

return euler;
}