卡尔曼滤波C++代码

 #include <ros/ros.h>
 #include <string>
 #include <stdlib.h>
 #include <iostream>
 #include <fstream>
 #include <string>
 #include <vector>
 #include <Eigen/Dense>
 #include <cmath>
 #include <limits>
 
 using namespace std;
 using Eigen::MatrixXd;
 
 //
 double generateGaussianNoise(double mu, double sigma)
 {
     const double epsilon = std::numeric_limits<double>::min();
     const double two_pi = 2.0*3.14159265358979323846;
 
     static double z0, z1;
     static bool generate;
     generate = !generate;
 
     if (!generate)
         return z1 * sigma + mu;
 
     double u1, u2;
     do
     {
         u1 = rand() * (1.0 / RAND_MAX);
         u2 = rand() * (1.0 / RAND_MAX);
     } while (u1 <= epsilon);
 
     z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);
     z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);
     return z0 * sigma + mu;
 }
 
 int main(int argc, char **argv)
 {
     std::cout<<"test qusetion2 start !"<<std::endl;
     ros::init(argc,argv,"question2_node");
     ros::NodeHandle node;
 
     ofstream fout("/media/kuang/code_test/result.txt");
 
     double generateGaussianNoise(double mu, double sigma);//随机高斯分布数列生成器函数
 
     const double delta_t = 0.1;//控制周期，100ms
     const int num = ;//迭代次数
     const double acc = ;//加速度，ft/m
 
     MatrixXd A(, );
     A(, ) = ;
     A(, ) = ;
     A(, ) = delta_t;
     A(, ) = ;
 
     MatrixXd B(, );
     B(, ) = pow(delta_t, ) / ;
     B(, ) = delta_t;
 
     MatrixXd H(, );//测量的是小车的位移，速度为0
     H(, ) = ;
     H(, ) = ;
 
     MatrixXd Q(, );//过程激励噪声协方差，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0
     Q(, ) = ;
     Q(, ) = ;
     Q(, ) = ;
     Q(, ) = 0.01;
 
     MatrixXd R(, );//观测噪声协方差，测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身。
     R(, ) = ;
 
     //time初始化，产生时间序列
     vector<double> time(, );
     for (decltype(time.size()) i = ; i != num; ++i) {
         time[i] = i * delta_t;
         //cout<<time[i]<<endl;
     }
 
     MatrixXd X_real(, );
     vector<MatrixXd> x_real, rand;
     //生成高斯分布的随机数
     for (int i = ; i<; ++i) {
         MatrixXd a(, );
         a(, ) = generateGaussianNoise(, sqrt());
         rand.push_back(a);
     }
     //生成真实的位移值
     for (int i = ; i < num; ++i) {
         X_real(, ) = 0.5 * acc * pow(time[i], );
         X_real(, ) = ;
         x_real.push_back(X_real);
     }
 
     //变量定义，包括状态预测值，状态估计值，测量值，预测状态与真实状态的协方差矩阵，估计状态和真实状态的协方差矩阵，初始值均为零
     MatrixXd X_evlt = MatrixXd::Constant(, , ), X_pdct = MatrixXd::Constant(, , ), Z_meas = MatrixXd::Constant(, , ),
         Pk = MatrixXd::Constant(, , ), Pk_p = MatrixXd::Constant(, , ), K = MatrixXd::Constant(, , );
     vector<MatrixXd> x_evlt, x_pdct, z_meas, pk, pk_p, k;
     x_evlt.push_back(X_evlt);
     x_pdct.push_back(X_pdct);
     z_meas.push_back(Z_meas);
     pk.push_back(Pk);
     pk_p.push_back(Pk_p);
     k.push_back(K);
 
     //开始迭代
     for (int i = ; i < num; ++i) {
         //预测值
         X_pdct = A * x_evlt[i - ] + B * acc;
         x_pdct.push_back(X_pdct);
         //预测状态与真实状态的协方差矩阵，Pk'
         Pk_p = A * pk[i - ] * A.transpose() + Q;
         pk_p.push_back(Pk_p);
         //K:2x1
         MatrixXd tmp(, );
         tmp = H * pk_p[i] * H.transpose() + R;
         K = pk_p[i] * H.transpose() * tmp.inverse();
         k.push_back(K);
         //测量值z
         Z_meas = H * x_real[i] + rand[i];
         z_meas.push_back(Z_meas);
         //估计值
         X_evlt = x_pdct[i] + k[i] * (z_meas[i] - H * x_pdct[i]);
         x_evlt.push_back(X_evlt);
         //估计状态和真实状态的协方差矩阵，Pk
         Pk = (MatrixXd::Identity(, ) - k[i] * H) * pk_p[i];
         pk.push_back(Pk);
     }
 
     cout << "含噪声测量" << "  " << "后验估计" << "  " << "真值" << "  " << endl;
     for (int i = ; i < num; ++i) {
         cout<<z_meas[i]<<"  "<<x_evlt[i](,)<<"  "<<x_real[i](,)<<endl;
         fout << z_meas[i] << "  " << x_evlt[i](, ) << "  " << x_real[i](, ) << endl;
     }
 
     fout.close();
     getchar();
     return ;
 }
 
下面是python画图代码：

 #! /usr/bin/env python
 
 import os
 import math
 import matplotlib.pyplot as plt
 import numpy as np
 
 def mean(data):
     sum=0
     for i in data:
         sum = sum+i
     return sum/len(data) 
 
 if __name__ ==  "__main__":
 
     file1 = "/media/kuang/code_test/result.txt"
 
     frame=[]
     measure_noise = []
     post_eval = []
     groundtruth = []
     counter = 0
 
     # Read all data
     document1 = open(file1,'rw+')
     for line1 in document1:
         split_line1 = line1.split()
         counter = counter + 1
         frame.append(counter)
         measure_noise.append(float(split_line1[0]))
     post_eval.append(float(split_line1[1]))
         groundtruth.append(float(split_line1[2]))
     document1.close()
 
     START = 0
     END = len(frame)-1 #
     frame_sub = frame[START:END]
     measure_noise_sub = measure_noise[START:END]
     post_eval_sub = post_eval[START:END]
     groundtruth_sub = groundtruth[START:END]
 
     fig, ax1 = plt.subplots()
 
     color = 'blue'
     ax1.set_xlabel('Frame No.')
     ax1.set_ylabel('measure noise', color=color)
     ax1.set_ylim(-10,500)
     ax1.plot(frame_sub, measure_noise_sub,'s-', color=color)
     ax1.tick_params(axis='y', labelcolor=color)
 
     ax2 = ax1.twinx()  # instantiate a second axes that shares the same x-axis
     color = 'red'
     ax2.set_ylabel('post eval', labelpad=40,color=color)  # we already handled the x-label with ax1
     ax2.set_ylim(-10,500)
     ax2.plot(frame_sub, post_eval_sub, 's-',color=color)
     ax2.tick_params(axis='y',pad=30.0, labelcolor=color)
 
     ax3 = ax1.twinx()
     color = 'yellow'
     ax3.set_ylabel('groundtruth', color=color, labelpad=30)
     ax3.set_ylim(-10,500)
     ax3.tick_params(axis='y', labelcolor=color)
     ax3.plot(frame_sub, groundtruth_sub, 's-' , color=color)
 
     fig.tight_layout()  # otherwise the right y-label is slightly clipped
     ax1.grid()
     plt.title("Kalman Test")
     plt.show()
 
     # SAVE RESULT TO FILE