implementation problem of my EKF program in C++ [closed]

edit

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/Exte...

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_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 ...