#include "imu.h"
#include <iostream>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<string>
#include<vector>
#include <cmath>
#include <limits>//用于生成随机分布数列
#include <stdlib.h>
#include <random>
#include <fstream>
#include <cstdlib> // 标准库
using namespace std;
using namespace Eigen;
using Eigen::MatrixXd;
int t=1;
double theta_PI=57.29578;
/*  定义系数*/
MatrixXd A(2,2);
MatrixXd B(2,1);
MatrixXd Qk(2,2);
MatrixXd H(1,2);
MatrixXd R(1,1);
MatrixXd I=MatrixXd::Identity(2,2);
Eigen::MatrixXd angle_1(1,1);


vector<MatrixXd> X_k;MatrixXd Xk=MatrixXd::Constant(2,1,0);
vector<MatrixXd> X_k_1;MatrixXd Xk_1=MatrixXd::Constant(2,1,0);

vector<MatrixXd> P_k;MatrixXd Pk=MatrixXd::Constant(2,2,0);
vector<MatrixXd> P_k_1;MatrixXd Pk_1=MatrixXd::Constant(2,2,0);
vector<MatrixXd> K_K;MatrixXd K=MatrixXd::Constant(2,2,0);
vector<MatrixXd> Y_K;MatrixXd Yk=MatrixXd::Constant(1,1,0);



class Kalman
{

public:


    Kalman()
    {
        IMU imu;
        //A<<1,imu.dt,0,1;
        //B<<imu.dt,0;
        Qk<<0.03,0,0,0.003;//QK的值暂定一个常量，后续需要求解角度方差和角速度偏差方差
        H(0,0)=1;H(0,1)=0;
        R<<3;//测量噪声,暂时给一个定值


        X_k.push_back(Xk);
        X_k_1.push_back(Xk_1);
        P_k.push_back(Pk);//此时刻的PK
        P_k_1.push_back(Pk_1);//上一时刻的PK
        K_K.push_back(K);
        Y_K.push_back(Yk);


        imu_sub=nd.subscribe<sensor_msgs::Imu>("imu", 100, &Kalman::IMUHandle,this);
        position_pub=nd.advertise<sensor_msgs::Imu>("imu_position",1000);
    }

  void IMUHandle(const sensor_msgs::Imu::ConstPtr &imu_measure)
  {
      IMU Tx;
      Tx.angle_v_x=imu_measure->angular_velocity.x;
      Tx.angle_v_y=imu_measure->angular_velocity.y;
      Tx.angle_v_z=imu_measure->angular_velocity.z;


      Tx.acc_x=imu_measure->linear_acceleration.x;
      Tx.acc_y=imu_measure->linear_acceleration.y;
      Tx.acc_z=imu_measure->linear_acceleration.z;
      double angle=-atan2(Tx.acc_x,sqrt(Tx.acc_y*Tx.acc_y+Tx.acc_z*Tx.acc_z));

      double angle_1=angle*theta_PI;

      double angle_v=Tx.angle_v_x;

    //  cout<<Tx.acc_x<<" "<<Tx.acc_y<<" "<<Tx.acc_z<<"         "<<angle_1<<endl;
      kalman_angle_x(angle,angle_v);


  }

  void kalman_angle_x(double angle,double angle_V)//angle是加速度计算出的角度 ,angle_V  角速度
  {

       IMU imu;
       angle_1(0,0)=angle;

       Xk_1(0,0)=(angle_V-imu.bias)*(t-1)*0.04;
       Xk_1(1,0)=imu.bias;
       X_k_1[0](0,0)=Xk_1(0,0);
       X_k_1[0](1,0)=Xk_1(1,0);



       A<<1,imu.dt*t,0,1;
       B<<imu.dt*t,0;
       Xk=A*X_k_1[t-1]+B*angle_V;
       X_k.push_back(Xk);



       Pk=A*P_k_1[t-1]*A.transpose()+Qk;
       P_k.push_back(Pk);



       MatrixXd tmp(1,1);
       tmp=H*P_k[t]*H.transpose()+R;
       K=P_k[t]*H.transpose()*tmp.inverse();
       K_K.push_back(K);


       Yk=angle_1-H*X_k[t];
       Y_K.push_back(Yk);


       Xk_1=X_k[t]+K_K[t]*Y_K[t];
       X_k_1.push_back(Xk_1);

       Pk=(I-K_K[t]*H)*P_k[t];
       P_k_1.push_back(Pk);
       cout<<X_k[t](0,0)-angle<<endl;
     //  cout<<angle<<endl;
       t++;

  }



private:
    ros::NodeHandle nd;
    ros::Publisher position_pub;
    ros::Subscriber imu_sub;



};

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

    Kalman PublicIMU;

    ros::spin();
    return 0;
}