卡尔曼滤波C++代码

  1 #include 
  2 #include <string>
  3 #include 
  4 #include 
  5 #include 
  6 #include <string>
  7 #include 
  8 #include 
  9 #include 
 10 #include 
 11  
 12 using namespace std;
 13 using Eigen::MatrixXd;
 14 
 15 //
 16 double generateGaussianNoise(double mu, double sigma)
 17 {
 18     const double epsilon = std::numeric_limits<double>::min();
 19     const double two_pi = 2.0*3.14159265358979323846;
 20  
 21     static double z0, z1;
 22     static bool generate;
 23     generate = !generate;
 24  
 25     if (!generate)
 26         return z1 * sigma + mu;
 27  
 28     double u1, u2;
 29     do
 30     {
 31         u1 = rand() * (1.0 / RAND_MAX);
 32         u2 = rand() * (1.0 / RAND_MAX);
 33     } while (u1 <= epsilon);
 34  
 35     z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);
 36     z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);
 37     return z0 * sigma + mu;
 38 }
 39 
 40 int main(int argc, char **argv)
 41 {
 42     std::cout<<"test qusetion2 start !"<<std::endl;
 43     ros::init(argc,argv,"question2_node");
 44     ros::NodeHandle node;
 45 
 46     ofstream fout("/media/kuang/code_test/result.txt");
 47     
 48     double generateGaussianNoise(double mu, double sigma);//随机高斯分布数列生成器函数
 49  
 50     const double delta_t = 0.1;//控制周期，100ms
 51     const int num = 100;//迭代次数
 52     const double acc = 10;//加速度，ft/m
 53  
 54     MatrixXd A(2, 2);
 55     A(0, 0) = 1;
 56     A(1, 0) = 0;
 57     A(0, 1) = delta_t;
 58     A(1, 1) = 1;
 59  
 60     MatrixXd B(2, 1);
 61     B(0, 0) = pow(delta_t, 2) / 2;
 62     B(1, 0) = delta_t;
 63  
 64     MatrixXd H(1, 2);//测量的是小车的位移，速度为0
 65     H(0, 0) = 1;
 66     H(0, 1) = 0;
 67  
 68     MatrixXd Q(2, 2);//过程激励噪声协方差，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0
 69     Q(0, 0) = 0;
 70     Q(1, 0) = 0;
 71     Q(0, 1) = 0;
 72     Q(1, 1) = 0.01;
 73  
 74     MatrixXd R(1, 1);//观测噪声协方差，测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身。
 75     R(0, 0) = 10;
 76  
 77     //time初始化，产生时间序列
 78     vector<double> time(100, 0);
 79     for (decltype(time.size()) i = 0; i != num; ++i) {
 80         time[i] = i * delta_t;
 81         //cout<
 82     }
 83  
 84     MatrixXd X_real(2, 1);
 85     vector x_real, rand;
 86     //生成高斯分布的随机数
 87     for (int i = 0; i<100; ++i) {
 88         MatrixXd a(1, 1);
 89         a(0, 0) = generateGaussianNoise(0, sqrt(10));
 90         rand.push_back(a);
 91     }
 92     //生成真实的位移值
 93     for (int i = 0; i < num; ++i) {
 94         X_real(0, 0) = 0.5 * acc * pow(time[i], 2);
 95         X_real(1, 0) = 0;
 96         x_real.push_back(X_real);
 97     }
 98  
 99     //变量定义，包括状态预测值，状态估计值，测量值，预测状态与真实状态的协方差矩阵，估计状态和真实状态的协方差矩阵，初始值均为零
100     MatrixXd X_evlt = MatrixXd::Constant(2, 1, 0), X_pdct = MatrixXd::Constant(2, 1, 0), Z_meas = MatrixXd::Constant(1, 1, 0),
101         Pk = MatrixXd::Constant(2, 2, 0), Pk_p = MatrixXd::Constant(2, 2, 0), K = MatrixXd::Constant(2, 1, 0);
102     vector x_evlt, x_pdct, z_meas, pk, pk_p, k;
103     x_evlt.push_back(X_evlt);
104     x_pdct.push_back(X_pdct);
105     z_meas.push_back(Z_meas);
106     pk.push_back(Pk);
107     pk_p.push_back(Pk_p);
108     k.push_back(K);
109  
110     //开始迭代
111     for (int i = 1; i < num; ++i) {
112         //预测值
113         X_pdct = A * x_evlt[i - 1] + B * acc;
114         x_pdct.push_back(X_pdct);
115         //预测状态与真实状态的协方差矩阵，Pk'
116         Pk_p = A * pk[i - 1] * A.transpose() + Q;
117         pk_p.push_back(Pk_p);
118         //K:2x1
119         MatrixXd tmp(1, 1);
120         tmp = H * pk_p[i] * H.transpose() + R;
121         K = pk_p[i] * H.transpose() * tmp.inverse();
122         k.push_back(K);
123         //测量值z
124         Z_meas = H * x_real[i] + rand[i];
125         z_meas.push_back(Z_meas);
126         //估计值
127         X_evlt = x_pdct[i] + k[i] * (z_meas[i] - H * x_pdct[i]);
128         x_evlt.push_back(X_evlt);
129         //估计状态和真实状态的协方差矩阵，Pk
130         Pk = (MatrixXd::Identity(2, 2) - k[i] * H) * pk_p[i];
131         pk.push_back(Pk);
132     }
133  
134     cout << "含噪声测量" << "  " << "后验估计" << "  " << "真值" << "  " << endl;
135     for (int i = 0; i < num; ++i) {
136         cout<"  "<0,0)<<"  "<0,0)<<endl;
137         fout << z_meas[i] << "  " << x_evlt[i](0, 0) << "  " << x_real[i](0, 0) << endl;
138     }
139  
140     fout.close();
141     getchar();
142     return 0;
143 }

144  


下面是python画图代码：

 1 #! /usr/bin/env python
 2 
 3 import os
 4 import math
 5 import matplotlib.pyplot as plt
 6 import numpy as np
 7 
 8 def mean(data):
 9     sum=0
10     for i in data:
11         sum = sum+i
12     return sum/len(data) 
13 
14 if __name__ ==  "__main__":
15 
16     file1 = "/media/kuang/code_test/result.txt"
17 
18     frame=[]
19     measure_noise = []
20     post_eval = []
21     groundtruth = [] 
22     counter = 0
23 
24     # Read all data
25     document1 = open(file1,'rw+')
26     for line1 in document1:
27         split_line1 = line1.split()
28         counter = counter + 1 
29         frame.append(counter)
30         measure_noise.append(float(split_line1[0]))
31     post_eval.append(float(split_line1[1]))
32         groundtruth.append(float(split_line1[2]))
33     document1.close()
34 
35     START = 0
36     END = len(frame)-1 # 1000
37     frame_sub = frame[START:END]
38     measure_noise_sub = measure_noise[START:END]
39     post_eval_sub = post_eval[START:END]
40     groundtruth_sub = groundtruth[START:END]
41     
42     fig, ax1 = plt.subplots()
43 
44     color = 'blue'
45     ax1.set_xlabel('Frame No.')
46     ax1.set_ylabel('measure noise', color=color)
47     ax1.set_ylim(-10,500)
48     ax1.plot(frame_sub, measure_noise_sub,'s-', color=color)
49     ax1.tick_params(axis='y', labelcolor=color)
50     
51     
52     ax2 = ax1.twinx()  # instantiate a second axes that shares the same x-axis
53     color = 'red'
54     ax2.set_ylabel('post eval', labelpad=40,color=color)  # we already handled the x-label with ax1
55     ax2.set_ylim(-10,500)
56     ax2.plot(frame_sub, post_eval_sub, 's-',color=color)
57     ax2.tick_params(axis='y',pad=30.0, labelcolor=color)
58 
59     ax3 = ax1.twinx()
60     color = 'yellow'
61     ax3.set_ylabel('groundtruth', color=color, labelpad=30)
62     ax3.set_ylim(-10,500)
63     ax3.tick_params(axis='y', labelcolor=color)
64     ax3.plot(frame_sub, groundtruth_sub, 's-' , color=color)
65     
66 
67     fig.tight_layout()  # otherwise the right y-label is slightly clipped
68     ax1.grid()
69     plt.title("Kalman Test")
70     plt.show()
71 
72     # SAVE RESULT TO FILE