卡尔曼滤波解析与算法研究

卡尔曼滤波是什么

卡尔曼滤波器最早要追寻到1960年卡尔曼先生发表的关于利用递归算法来求解离散线性滤波器问题的学术论文——《A New Approach to Linear Filtering and Prediction Problems》(线性滤波与预 测问题的新方法)，如果对这编论文有兴趣，可以到这里的地址下载： http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf在卡尔曼先生的这篇学术论文中 首次提出了针对维纳滤波器缺点的全新解决方案， 这种方案就是现在仍在广泛用于数据处理以及除噪声领域的卡尔曼滤波 方法，也就是说卡尔曼滤波是对维纳滤波器的改进版本。那么就不得不提一提维纳滤波器是什么了
     维纳滤波是什么
  维纳滤波（wiener filtering) 一种基于最小均方误差准则、对平稳过程的最优估计器。这种滤波器的输出与期望输出的均方误差为最小，因此，它是一个最佳滤波系统。它可用于提取被平稳噪声所污染的信号。         维纳滤波实现的要求 
    1.输入过程是广义平稳的

2.输入过程的统计特性是已知的。根据其他最佳准则的滤波器亦有同样要求

        oh no!!!!   是不是觉得巨TM的坑爹，要知道，我们在现实生活中的噪声的统计特性都是不确定的呀，要是能够确定那叫其他随机    时间序列的信号源好吗？？？再说生活中基本都是非平稳的噪声呀，都是非线性系统下或者线性混合系统的噪声。但是呀，正是有这样坑爹的滤波器我们才会催生其他滤波器呀。于是新一代的自适应滤波器（卡尔曼滤波）拔地而起。
（抱歉，这里并不是说维纳滤波不伟大，维纳滤波最主要的贡献就是提供了最小均方误差的准则）

[align=left]卡尔曼滤波做的改进      最小均方误差被用来作为卡尔曼滤波器估计的准则，在完成对信号状态变量的估计时，信号与噪声 的状态空间模型是卡尔曼滤波算法首先使用的一个 模型，在进行状态变量的估计时，利用的是当前时刻 的估计值和当前时刻的观测值。[/align][align=left]标准卡尔曼滤波算法推导        首先给出控制理论的两个公式：[/align][align=left]                  状态方程：x(k)=A(k)*x(k-1)+w(k-1)[/align]                    测量方程：y(k)=c(k)*x(k)+v(k)  再给出我们的两个基本问题：1.卡尔曼滤波要做什么？对状态进行预测，估计2.卡尔曼滤波怎么对状态进行估计？利用状态过程噪声和测量噪声的最小均方差误差最小来对状态进行估计。                      过程噪声：目标对象再演化过程中遭遇的干扰。                      测量噪声：对目标对象测量时侯的外界干扰。      
3.卡尔曼滤波原理：        一个状态在一个时刻点k的状态进入下一个时刻点k+1状态，会有很多外界因素的干 扰，  我们把干扰就叫做过程噪声，用w表示。任何一个测量仪器，都会有误差，我们把这个误差叫做量测噪声，用v表示。 回到上面那个公式，状态方程表示状态在不断的更新，从一个时刻点进入下一个时刻点，这个很好理解。关键是量测方程，它表示，我们不断更新的状态有几个能用测量仪器测出来，比如，汽车运动状态参数有很多，比如速度，轮速，滑移率等，但是我们只能测量出轮速，因此量测方程要做的就是把状态参数中能量测的状态拿出来。 我们始终要记得我们要做的事：我们要得到的是优化的状态量X（k）。 理解了上面之后就可以开始推导公式了。        4.卡尔曼滤波公式推导：（以下部分部分来自知乎）          1.首先不考虑过程噪声对状态进行更新，很简单：

举个例子，v(k)=v(k-1)+at，匀加速运动咯。（这里的A变换矩阵）2.不考虑测量噪声取出能测量的状态，也很简单：

（C(K)A(K)也是变换矩阵）3.用测量仪器测量出来的状态值（大家可以考虑到：测量的值就是被各种噪声干扰后的真实值）减去上面不考虑噪声得到的测量值：

这个值在数学上是一个定义值，叫做新息，有很多有趣的性质，感兴趣的可以自己谷歌。我们对步骤暂且停一停。这个叫新息的值有什么用？由上面的过程我们可以明显看到，它反映了过程噪声和测量噪声综合对测量状态值的影响，也就是它包含了w和v的情况。（也就是说新息是由过程噪声和测量噪声组成的）一个数值c由两部分内容a和b组成，那么怎样用数学表达式来表达？ 一般有两种做法： I.直接相加：c=a+b;II. 用比例的方法：a=n*c,b=(1-n)*c卡尔曼滤波用了方法二，也就是给两个噪声各自乘以一个权重，一般而言过程噪声的权重大于测量噪声。就是说，过程噪声w=新息*一个比例。这样得到的过程噪声加上原来（第一步）不考虑过程噪声的状态值不就是优化值了吗？ 也就是：

Okay，都写到这里了，有必要做一下前提假设：a. 什么高斯噪声，均值为零一堆；b.Ak,Ck,wk的协方差Q,vk的协方差R，系统协方差初始值P0,状态初始值X0，都已知。为什么已知，这是你的状态参数，打个比方，如果你用温度计测量室内温度，你别告诉我你不知道每一度水银柱上升多少CM。
那么到目前为止我们的思路就是清楚了，找到一个合适的Hk值（卡尔曼增益），那么我们就能得到状态的最优值。（卡尔曼说的，不是我说的，所以你问为什么，你要问他，这么深层次的理论留给博士和学者们去做就好，我们就现学现用就行，哈哈哈，站在巨人的肩膀！） 问题来了：怎么得到合适的Hk？似乎不是随便一个参数。

这是误差协方差矩阵。
思路：使得误差协方差矩阵Pk最小的Hk。（如何实现呢？不知道！ 懂了之后补充）协方差表示什么，协方差表示两者之间的联系或者关系，关系越大，协方差越大。误差协方差越小说明过程噪声和量测噪声的关系越小。关系越小能做什么，这要回到我们第3步讨论的我们用比例的方法分开了w和v。用比例分开，到底多少属于w，多少是v，如果关系越小，分开的越精确，比如一堆白砂糖和盐，如果两种混合的很均匀，我们说它关系很大，也就越难用比例的方法将其分开。基于MATLAB的卡尔曼滤波算法的实现：（居然没有matlab语言格式）

clear
N=200;
w(1)=0;
w=randn(1,N)
x(1)=0;
a=1;
for k=2:N;
x(k)=a*x(k-1)+w(k-1);
end

V=randn(1,N);
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2;
q3=std(w);
Rww=q3.^2;
c=0.2;
Y=c*x+V;

p(1)=0;
s(1)=0;
for t=2:N;
p1(t)=a.^2*p(t-1)+Rww;
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
p(t)=p1(t)-c*b(t)*p1(t);
end

t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');

仿真的环境于状态：假设房间的真实温度为25度，我模拟了200个测量值，这些测量值的平均值为25度，但是加入了标准偏差为几度的高斯白噪声（在图中为蓝线）
为了令卡尔曼滤波器开始工作，我们需要告诉卡尔曼两个零时刻的初始值，是X(0|0)和P(0|0)。他们的值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，X会逐渐的收敛。但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。我选了X(0|0)=1度，P(0|0)=10。

该系统的真实温度为25度，图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果（该结果在算法中设置了Q=1e-6，R=1e-1）。



欢迎交流！！！

[align=left]
[/align]