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 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.pitchekf:89.7621 data.yawekf: 85.745 data.roll_ekf:-0.719216
data.pitchsens:-1.59499 data.yawsens: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;
}
Asked by sasadasd on 2017-08-21 05:00:16 UTC
Comments
This seems like it may be better suited for Robotics Stack Exchange.
Asked by jayess on 2017-08-21 16:11:34 UTC
Thank you for your advice . I haven't ever known the web site and I will try to ask my question in the web site later .
Asked by sasadasd on 2017-08-22 07:19:45 UTC