#include <iostream>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<string>
#include<vector>
#include <cmath>
#include <limits>//用于生成随机分布数列
#include <stdlib.h>
#include <random>
using namespace std;
using namespace Eigen;
using Eigen::MatrixXd;

int main()
{
      const double dt=0.1;//采样时间
      const int MAX=500;//最大迭代次数
      double acc=0.2;//加速度 也是公式中的uk
      double theta_t;
      vector<MatrixXd> X_now;//实际位移矩阵存入这个容器中
      /*第一个公式中的变量矩阵/容器*/
      vector<MatrixXd> X_k;MatrixXd Xk=MatrixXd::Constant(2,1,0);X_k.push_back(Xk);
      vector<MatrixXd> X_k_1;MatrixXd Xk_1=MatrixXd::Constant(2,1,0);X_k_1.push_back(Xk_1);

      /*第二个公式中的变量矩阵/容器*/
      vector<MatrixXd> P_k;MatrixXd Pk=MatrixXd::Constant(2,2,0);P_k.push_back(Pk);
      vector<MatrixXd> P_k_1;MatrixXd Pk_1=MatrixXd::Constant(2,2,0);P_k_1.push_back(Pk_1);

      /*第三个公式中的变量矩阵/容器*/
      vector<MatrixXd> K_K;MatrixXd K=MatrixXd::Constant(2,2,0);K_K.push_back(K);

       /*第四个公式中的变量矩阵/容器*/
      vector<MatrixXd> Z_measure;MatrixXd Zk=MatrixXd::Constant(1,1,0);Z_measure.push_back(Zk);

      /*公式五里没有新的变量矩阵*/

      //把系数确定好
      MatrixXd A(2,2);

      MatrixXd B(2,1);
      MatrixXd H(1,2);

      A(0,0)=1;A(0,1)=dt;A(1,0)=0;A(1,1)=1;

      B(0,0)=(dt*dt)/2;B(1,0)=dt;

      H(0,0)=1;H(0,1)=0;


      //把噪声以及固定值确定好
      MatrixXd Q(2,2);//过程激励噪声协方差，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0
      Q(0,0) = 0;
      Q(1,0) = 0;
      Q(0,1) = 0;
      Q(1,1) = 0.01;

      MatrixXd R(1,1);
      R<<10;

      MatrixXd I=MatrixXd::Identity(2,2);

      //确定变量矩阵
      MatrixXd X(2,1);//实际位移值

      double noise_random;//随即噪声


      //定义时间信息tehta_t
      vector<double> time(1000, 0);
     // int i;
      for(int i = 0; i < MAX; i++)
      {
          time[i] = i * dt;
         // cout<<time[i]<<endl;
          theta_t =time[i];


         X(0,0)=1/2*acc*theta_t*theta_t  ; //实际位移，以后会用来求测量值Zk
          X(1,0)=0;
          X_now.push_back(X);
       }
      /*开始进入迭代*/
      for(int i = 1; i < MAX; i++)
      {



         Xk=A*X_k_1[i-1]+B*acc;
         X_k.push_back(Xk);


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


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

         Zk=H*X_now[i];
         Z_measure.push_back(Zk);
         Xk_1=X_k[i]+K_K[i]*(Z_measure[i]-H*X_k[i]);
         X_k_1.push_back(Xk_1);


         Pk=(I-K_K[i]*H)*P_k[i];
         P_k_1.push_back(Pk);


      }



}