卡爾曼濾波C++代碼

 1 #include <ros/ros.h>
 2 #include <string>
 3 #include <stdlib.h>
 4 #include <iostream>
 5 #include <fstream>
 6 #include <string>
 7 #include <vector>
 8 #include <Eigen/Dense>
 9 #include <cmath>
 10 #include <limits>
 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<<time[i]<<endl;
 82  }  83  
 84     MatrixXd X_real(2, 1);  85     vector<MatrixXd> 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<MatrixXd> 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<<z_meas[i]<<"  "<<x_evlt[i](0,0)<<"  "<<x_real[i](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