学习卡尔曼滤波

偶然机会想学学（最起码知道一下）卡尔曼滤波，以前学人工智能课程学过滤波算法，但是老师只是提了一下有这个东西，一直不知道如何用它来解决问题。

1，参考资料：http://www.cs.unc.edu/~welch/kalman/
里面有一个"An Introduction to the Kalman Filter
"，这个也有中文版，但是需要翻强，很多代理道行不够翻不过去，用这个http://www.webproxyserver.cn/
（这个代理服务器居然变成里人体艺术的链接了，无语，看来还得去找一个其他的）来翻这个http://sites.google.com/site/yaoxuchen/Home/kalman
。看这些至少应该有一些概率方面的基础，还要知道一些随即过程和概率推理的知识。

2，结合"An Introduction to the Kalman Filter
"，参考里面的例子，自己也动手写一写，加深印象，例子的python实现http://www.scipy.org/Cookbook/KalmanFiltering
看这个。下面是一个C＋＋的实现：没有注释，几个变量根据"An Introduction to the Kalman Filter
"设置的，看完这篇文章，程序就没问题了（还会发现我写的很烂）。需要稍微解释一下的就是测量值加高斯噪声是使用两个一致分布产生正态分布的方法实现的。最后结果使用gnuplot来画图很方便。结果是没有问题的。

#include <fstream>
#include <cstdlib>
#include <ctime>
#include <cmath>
using namespace std;
const int INTERATION_NUM = 50;
const double VALUE = -0.37727;
double x = 0;
double x_p = 0;
double Q = 1e-5;
double P_p = 1;
double R = 0.1*0.1;
double I = 1;
double K = 0;
double measure[INTERATION_NUM];
ofstream out_measure("measure.txt");
ofstream out_kalman("kalman.txt");
ofstream out_err_p("err_p.txt");
double normal(double mean,double stdev){
double nu = (double)rand()/RAND_MAX;
double mu = (double)rand()/RAND_MAX;
double res = sqrt(-2*log(nu))*cos(2*M_PI*mu)*stdev+mean;
return res;
}
void filter_iteration(int times){
for(int i=0;i<times;i++){
//time update.
x_p = x;
P_p = P+Q;
if(i!=0)
out_err_p<<i<<" "<<P<<endl;
//measement update.
K = P_p/(P_p + R);
x = x_p + K*(measure[i] - x_p);
P = (I - K)*P_p;
out_kalman<<i<<" "<<x<<endl;
}
}
int main(){
srand((unsigned)time(NULL));
for(int i=0;i<INTERATION_NUM;i++){
measure[i] = normal(0,0.1) + VALUE;
out_measure<<i<<" "<<measure[i]<<endl;
}
filter_iteration(INTERATION_NUM);
return 0;
}

附：kalman滤波的一个实现：mit实验室http://web.media.mit.edu/~wad/mas864/psrc/
来自：

Modeling Camera Motion（http://web.media.mit.edu/~wad/mas864/）