卡尔曼滤波器及其简matlab仿真.docx
《卡尔曼滤波器及其简matlab仿真.docx》由会员分享，可在线阅读，更多相关《卡尔曼滤波器及其简matlab仿真.docx（8页珍藏版）》请在冰豆网上搜索。
卡尔曼滤波器及其简matlab仿真
卡尔曼滤波器及其简matlab仿真
一、卡尔曼滤波的起源
谈到信号的分析与处理，就离不开滤波两个字。
通常，信号的频谱处于有限的频率范围内，而噪声的频谱则散布在很广的频率范围内，为了消除噪声，可以进行频域滤波。
但在许多应用场合，需要直接进行时域滤波，从带噪声的信号中提取有用信号。
虽然这样的过程其实也算是对信号的滤波，但其所依据的理论，即针对随机信号的估计理论，是自成体系的。
人们对于随机信号干扰下的有用信号不能“确知”，只能“估计”。
为了“估计”，要事先确定某种准则以评定估计的好坏程度。
1960年卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文A New Approach to Linear Filtering and Prediction Problems（线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，这就是我们今天称之为卡尔曼滤波的方法。
卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态甚至能估计将来的状态即使并不知道模型的确切性质。
其基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值。
算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。
对于解决很大部分的问题，它是最优，效率最高甚至是最有用的。
它的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。
近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。
卡尔曼滤波不要求保存过去的测量数据，当新的数据到来时，根据新的数据和前一时刻的储值的估计，借助于系统本身的状态转移方程，按照一套递推公式，即可算出新的估值。
卡尔曼递推算法大大减少了滤波装置的存储量和计算量，并且突破了平稳随机过程的限制，使卡尔曼滤波器适用于对时变信号的实时处理。
二、卡尔曼滤波的原理
卡尔曼滤波思想的来源是在海图作业中，航海长通常以前一时刻的船位为基准，根据航向、船速和海流等一系列因素推算下一个船位，但是他并不轻易认为船位就一定在推算船位上，还要选择适当的方法，通过仪器得到另一个推算船位。
观测和推算这两个船位一般不重合，航海长需要通过分析和判断选择一个可靠的船位，作为船舰当前的位置。
就是以现时刻的最佳估计为在前一时刻的最佳估计的基础上根据现时刻的观测值作线性修正。
卡尔曼滤波在数学上是一种线性最小方差统计估算方法，它是通过处理一系列带有误差的实际测量数据而得到物理参数的最佳估算。
其实质要解决的问题是要寻找在最小均方误差下
的估计值
。
它的特点是可以用递推的方法计算
，其所需数据存储量较小，便于进行实时处理。
具体来说，卡尔曼滤波就是要用预测方程和测量方程对系统状态进行估计。
设动态系统的状态方程和测量方程分别为：
上两式子中，
是k时刻的系统状态，
和
是k-1时刻到k时刻的状态转移矩阵，
是k时刻的测量值，
是测量系统的参数，
和
分别表示过程和测量的噪声，他们被假设成高斯白噪声。
如果被估计状态和观测量是满足上述第一式，系统过程噪声和观测噪声满足第二式的假设，k时刻的观测
的估计
可按下述方程求解。
状态的一步预测：
（1）
均方误差进一步预测：
（2）
滤波增益矩阵：
（3）
滤波估计方程：
（4）
均方误差更新矩阵（K时刻的最优均方误差）：
（5）
上述就是卡尔曼滤波器的5条基本公式，只有给定初值
和
，根据k时刻
的观测值
，就可以递推计算得k时刻的状态估计
。
再把有关
的项归在平方项里，即
（12）
在式（12）中，欲使
极小，则
（13）
此时
（14）
这就是误差迭代公式。
由此可见，卡尔曼滤波的递推公式即可得到滤波的估计值，又可得到误差的方差阵。
由式（3）两边同时减去
得：
（15）
将式
（1），式（5）带入式（15）
因此
（20）
式（20）即一步预测估计误差矩阵。
到此，卡尔曼滤波公式推导完毕。
三、卡尔曼滤波的实例——以温度检测滤波为例
假设我们要研究一个房间的温度，这个房间的真实温度是25度，以一分钟为时间单位。
根据我们的经验判断，这个房间的温度是恒定的（A=1），但是对我们的经验不是完全相信，可能存在上下几度的偏差，我们把该偏差看做是高斯白噪声（系统噪声W）。
另外，我们在房间里放一个温度计（H=1），温度计也不准确，测量值会与实际值存在偏差，我们也把这偏差看做是高斯白噪声（测量噪声V）。
现在我们用卡尔曼滤波，过滤掉噪声，估算出房间的实际温度。
系统参数名称解释如下：
xk
系统状态
实际温度
系统矩阵
温度不变，为1
B、uk
状态的控制量
无控制量，为0
Zk
观测值
温度计读数
H
观测矩阵
直接读出，为1
wk
过程噪声
温度变化偏差，常量1e-1
vk
测量噪声
读数误差，常量1e-6
假如我们要估算2时刻房间的实际温度值。
首先你要根据1时刻温度的估计值（就假设为1），来算出2时刻温度的估计值，即：
=1（*公式1），然后由给出的1时刻的均方差（假设为10）进一步更新均方差，有=10.009（*公式2）。
有方差之后，根据卡尔曼增益方程计算出增益：
=0.925（*公式3）。
现在，我们可以通过增益和测量值（Z=26.676，温度计有测量误差）来估计2时刻的温度了，=24.763（*公式4）。
得到了2时刻的估计温度，下一步就是对3时刻的温度值进行最优估算，需要得到K时刻的最优温度（24.56）的偏差，算法如下：
=0.751（*公式5）
就这样，再进入3时刻的滤波循环，卡尔曼滤波器就不断的把均方误差递归，从而估算出最优的温度值，运行速度快。
以下是matlab程序：
clearall;
clc;
closeall;
%系统方程X（k）=AX（k-1）+BU（k）+W（k）
%系统测量值Z（k）=HX（k）+V（k）
N=200;
w=0.1*randn（1,N）;%产生随机高斯分布
x
（1）=0;%赋值与否无所谓
a=1;%系统状态矩阵
V=randn（1,N）;%产生随机高斯分布
q1=std（V）;
Rvv=q1.^2;%R
q2=std（w）;
Qww=q2.^2;%Q
h=1;%观测矩阵
Y=25+V;%测量误差值
p
（1）=10;
X
（1）=0;
%%%%%%%%%%kalmanfilter%%%%%%%%%%
%x（k）=x（k|k-1）,X（k）=x（k|k）,p1（k）=p（k|k-1）,p（k）=p（k|k）
fork=2:
N;
x（k）=a*X（k-1）;%X（k|k-1）=AX（k-1|k-1）+BU（k）………
（1）
p1（k）=a*p（k-1）*a'+Qww;%P（k|k-1）=AP（k-1|k-1）A’+Q………
（2）
Kg（k）=p1（k）*h'/（h*p1（k）*h'+Rvv）;%Kg（k）=P（k|k-1）H’/（HP（k|k-1）H’+R）………（4）
X（k）=x（k）+Kg（k）*（Y（k）-h*x（k））;%X（k|k）=X（k|k-1）+Kg（k）（Z（k）-HX（k|k-1））………（3）
p（k）=p1（k）-Kg（k）*h*p1（k）;%P（k|k）=（I-Kg（k）H）P（k|k-1）………（5）
end
fori=1:
N
expValue（i）=25;
end
k=1:
N;
plot（k,X,'r',k,Y,'g',k,expValue,'b'）;
legend（'真实值','估计值','测量值'）;
axis（[0N1030]）
xlabel（'时间'）;
ylabel（'房间温度'）;
title（'卡尔曼滤波'）;
滤波结果如下图：
由上图可以看出，滤波效果比较明显，滤波后的温度基本等于房间的真实值25度，将温度测量的大部分误差都过滤掉了。
从仿真结果可以看出，卡尔曼滤波器能够有效地在一定的噪声干扰下比较准确地反映真实值。
升级会员 