卡尔曼滤波器及matlab代码

1、信息融合大作业维纳最速下降法滤波器，卡尔曼滤波器设计及 Matlab 仿真时间： 2010-12-6 专业：信息工程 班级： 09030702 学号： 71姓名：马志强1. 滤波问题浅谈估计器或滤波器这一术语通常用来称呼一个系统， 设计这样的系统是为了从 含有噪声的数据中提取人们感兴趣的，接近规定质量的信息。由于这样一个宽 目标，估计理论应用于诸如通信、雷达、声纳、导航、地震学、生物医学工程、 金融工程等众多不同的领域。例如，考虑一个数字通信系统，其基本形式由发 射机、信道和接收机连接组成。发射机的作用是把数字源 （例如计算机 ）产生的 0、1 符号序列组成的消息信号变换成为适合于信道上传送的

2、波形。而由于符号 间干扰和噪声的存在， 信道输出端收到的信号是含有噪声的或失真的发送信号。 接收机的作用是，操作接收信号并把原消息信号的一个可靠估值传递给系统输 出端的某个用户。随着通信系统复杂度的提高，对原消息信号的还原成为通信 系统中最为重要的环节，而噪声是接收端需要排除的最主要的干扰，人们也设 计出了针对各种不同条件应用的滤波器，其中最速下降算法是一种古老的最优 化技术，而卡尔曼滤波器随着应用条件的精简成为了普适性的高效滤波器。 2维纳最速下降算法滤波器最速下降算法的基本思想考虑一个代价函数 ，它是某个未知向量 的连续可微分函数。函数将 的元素映射为实数。这里，我们要寻找一个最优解 。使

3、它满足如下条件这也是无约束最优化的数学表示。特别适合于自适应滤波的一类无约束最优化算法基于局部迭代下降的算法：从某一初始猜想 出发，产生一系列权向量 ，使得代价函数在算法的每一次迭代都是下降的，即其中 是权向量的过去值，而 是其更新值。我们希望算法最终收敛到最优值 。迭代下降的一种简单形式是最速下降 法，该方法是沿最速下降方向连续调整权向量。为方便起见，我们将梯度向量 表示为因此，最速下降法可以表示为其中 代表进程， 是正常数，称为步长参数， 1/2 因子的引入是为了数学上处 理方便。在从 到 的迭代中，权向量的调整量为为了证明最速下降算法满足式，在 处进行一阶泰勒展开，得到此式对于 较小时是

4、成立的。在式中设 为负值向量，因而梯度向量 也为负值 向量，所以使用埃尔米特转置。将式用到式中，得到此式表明当 为正数时， 。因此，随着 的增加，代价函数 减小，当 时，代价函数趋于最小值 。最速下降算法应用于维纳滤波器考虑一个横向滤波器，其抽头输入为 ，对 应的抽头权值为 。抽头输入是来自零均值、相关矩阵为 的广义平稳随机过程的抽样值。除了这些输入外，滤波器还要一个期望响应，以便为最优滤波提供一个参考。 在时刻 抽头输入向量表示为 ，滤波 器输出端期望响应的估计值为 , 其中 是由抽头输 所张成的空间。空过比较期望响应 及其估 计值，可以得到一个估计误差，即这里 是抽头权向量 与抽头输入向量

5、 的内积。 可以进一步 表示为同样，抽头输入向量 可表示为如果抽头输入向量 和期望响应是联合平稳的， 此时均方误差或者在时刻 的代价函数 是抽头权向量的二次函数，于是可以得到其中， 为目标函数 的方差， 抽头输入向量 与期望响应 的互相关向量，及 为抽头输入向量 的相关矩阵。从而梯度向量可以写为其中在列向量中 和 分别是代价函数 对应第 个抽头权值 的实部 和虚部 的偏导数。对最速下降算法应用而言，假设式中相关矩阵 和互相关向量 已知，则对于给定的抽头权向量 为它描述了为那滤波中最速下降法的数学表达式。3. 卡尔曼滤波器卡尔曼滤波器的基本思想卡尔曼滤波器是用状态空间概念描述其数学公式的，另外新

6、颖的特点是， 他的解递归运算，可以不加修改地应用于平稳和非平稳环境。尤其是，其状态 的每一次更新估计都由前一次估计和新的输入数据计算得到，因此只需存储前 一次估计。除了不需要存储过去的所有观测数据外，卡尔曼滤波计算比直接根 据滤波过程中每一步所有过去数据进行估值的方法都更加有效。“状态”的概念是这种表示的基础。状态向量，简单地说状态，定义为数据 的最小集合， 这组数据足以唯一地描述系统的自然动态行为。 换句话说， 状态由 预测系统未来特性时所素要的， 与系统的过去行为有关的最少的数据组成。 典型 地，比较有代表性的情况是，状态 是未知的。为了估计它，我们使用一组观 测数据，在途中用向量 表示。

7、 成为观测向量或者简称观测值， 并假设它 是 维的。在数学上，图表示的信号流图隐含着一下两个方程：(1) 过程方程式中， 向量 表示噪声过程，可建模为零均值的白噪声过程，且其相关矩阵定义为(2)测量方程其中 是已知的 测量矩阵。 向量 称为测量噪声，建模为零均值的白噪声过程，其相关矩阵为测量方程确立了可测系统输出 与状态 之间的关系，如图所示。新息过程为了求解卡尔曼滤波问题， 我们将应用基于新息过程的方法。 根据之前所述，用向量 表示 时刻到 时刻所有观测数据过去值给定的情况 下，你时刻观测数据 的最小均方估计。过去的值用观测值表示，他们张成的向量空间用 表示。从而可以定义 新息过程如下：其中

8、 向量 表示观测数据 的新息。应用新息过程进行状态估计下面，我们根据信息过程导出状态 的最小均方估计。 根据推导， 这个估 计可以表示成为新息过程 序列的线性组合，即其中 是一组待定的 矩阵。根据正交性原理， 预测状态误差向量 与新息过程正交，即将式代入式，并利用新息过程的正交性质，即得因此，式两边同时右乘逆矩阵 ，可得 的表达式为最后，将式带入式，可得最小军方差估计故对于 ，有然而， 时刻的状态 与 时刻的状态 的关系式由式可以推导出对 于 ，有其中 只与观测数据 有关。因此可知， 与 彼此正 交（其中） 。利用式以及当时 的计算公式，可将式右边的求和项改写为为了进一步讨论，引入如下基本定义

9、。卡尔曼增益定义 矩阵其中 是状态向量 和新息过程 的互相关矩阵。 利 用这一定义和式的结果，可以将式简单重写为式具有明确的物理意义。 它标明：线性动态系统状态的最小均方估计 可以由前一个估计 求得。为了表示对卡尔曼开创性贡献的认可，将矩 阵 称为卡尔曼增益。现在剩下唯一要解决的问题是， 怎样以一种便于计算的形式来表示卡尔曼增益 。为此，首先将 与 乘积的期望表示为式中利用了状态 与噪声向量 互不相关这一事实。其次，由于预测状态 误差向量 与估计 正交，因此 与 乘 机的期望为零。这样，用预测状态误差向量 代替相乘因子 ，将 不会引起式变化，故有由此，可将上式进一步变化为现在我们重新定义卡尔曼

10、增益。为此，将式代入式得现在我们已经了解了卡尔曼滤波的整个过程和相应的参数设置， 为了能够更为方便利用计算机仿真实现，特将其中参数变量进行小结。变量卡尔曼变量和参数小结定义维数时刻状态时刻状态值从 时刻到 时刻的转移矩阵时刻的测量矩阵过程噪声 的相关矩阵过程噪声 的相关矩阵给定观测值在 时刻状态的预测估计给定观测值 在时刻状态的滤波估计时刻卡尔曼增益矩阵时刻新息向量新息向量 的相关矩阵中误差相关矩阵中误差相关矩阵基于单步预测的卡尔曼滤波器的小结观测值=转移矩阵 =测量矩阵 =过程噪声的相关矩阵 =测量噪声的相关矩阵 =4 Matlab 仿真为了简化，这里只讨论简单的一维单输入单输出线性系统模型

11、， 其中加入白噪声作为系统的扰动，具体仿真结果可以获得如下维纳最速下降法滤波器仿真结果以上为最速下降法中不同的递归步长所导致的跟踪效果变化， 对于最速下降法中 的步长是影响其算法稳定的关键， 最速下降算法稳定的充分必要条件是条件步长 因子为小于输入自相关矩阵的最大特征值倒数的 2 倍。上面的序列分别从相关矩 阵的随大特征之 2 倍的倍开始变化至其 1 倍，最后一幅图象能够看出其已经不再 收敛，下面是大于输入相关矩阵的最大特征值 2 倍步长时所表现的跟踪结果可以看出其已经明显发散， 不再是我们所期望的滤波算法。 因此可以总结出， 对 于最速下降法来说， 步长的选取是很重要的， 根据不同条件的需求

12、， 选取正确的步长，能为算法的快速高效提供基础。卡尔曼滤波器仿真结果从图中可以发现，卡尔曼滤波器能够非常有效地在比较大的干扰下比较准确 地反映真实值， 如果观测端加入干扰较大时， 卡尔曼滤波器能够较为有效地进行 滤波，不过当状态端的干扰增大时， 卡尔曼滤波器的滤波效果也会随之下降。 如 下图，是加大了状态端的干扰，所呈现的滤波效果。如上图所示， 状态端的干扰导致状态不稳定， 卡尔曼滤波器的估计值也出现了比较大的波动。如果将状态端的干扰再增大，则会出现更为严峻的滤波考验， 滤波效果如下。这是的状态已经很勉强了， 所以，研究更为有效的多方法卡尔曼滤波器也显 得十分必要了。一种不需初始化的卡尔曼滤波

13、器仿真这种滤波器只是实现了无需对部分变量进行初始化的设计， 没有特别意义上的改进经典卡尔曼滤波器本身性能的特点。仿真图如下后联平滑滤波的卡尔曼滤波器仿真 只是在经典卡尔曼滤波器后端联接了平滑滤波器， 对性能改进的效果并不特 别明显，仿真图如下如图中所表示，即使平滑过的估值与观测值之间的差别也不是特别令人满 意，所以，对于经典卡尔曼滤波的研究还需要更深一步进行， 由于时间和能力有 限，本次的作业对于卡尔曼及其他滤波器的研究只能达到这种程度， 希望在以后 的学习中，能发现更好的对经典卡尔曼滤波器的改进方法。5 Matlab 源代码 ( 部分参考自互联网 ) 经典卡尔曼滤波器 clearN=200;

14、w(1)=0;x(1)=5;a=1;c=1;Q1 = randn(1,N)*1;% 过程噪声Q2 = randn(1,N);% 测量噪声for k=2:N;x(k)=a*x(k-1)+Q1(k-1); end%状态矩阵for k=1:N;Y(k)=c*x(k)+Q2(k);endp(1)=10;s(1)=1;for t=2:N;Rww=cov(Q1(1:t);Rvv=cov(Q2(1:t);p1(t)=a.2*p(t-1)+Rww;b(t)=c*p1(t)/(c.2*p1(t)+Rvv);%kalman 增益s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1);p(t)=p1

15、(t)-c*b(t)*p1(t);endt=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');% 红色卡尔曼，绿色观测值，蓝色状态值legend('kalman estimate','ovservations','truth');最速下降法clc clear all N=30;q=;%q>1&&q<2/Ryx最大特征值 hn=zeros(1,N);hn(:)=5; vg=0;Rxx=xcorr(1);Ryx=min(min(corrcoef(1, 1+r

16、andn); echo off for i=1:N-1;%vg=2*Rxx*hn(:,i)-2*Ryx; %hn(:,i+1)=hn(:,i)-1/2*q*vg;vg=2*Rxx*hn(i)-2*Ryx; hn(i+1)=hn(i)-1/2*q*vg;m(i)=1;end t=1:N-1; plot(t,hn(t),'r-',t,m(t),'b-');后联平滑滤波器的卡尔曼滤波器 clear clc;N=300; CON = 5;x = zeros(1,N);x(1) = 1; p = 10;Q = randn(1,N)*;% 过程噪声协方差R = randn(

17、1,N);% 观测噪声协方差 y = R + CON;%加过程噪声的状态输出 for k = 2 : NQ1 = cov(Q(1:k-1);%过程噪声协方差Q2 = cov(R(1:k-1);x(k) = x(k - 1);%预估计 k 时刻状态变量的值p = p + Q1;% 对应于预估值的协方差kg = p / (p + Q2);%kalman gainx(k) = x(k) + kg * (y(k) - x(k);p = (1 - kg) * p;endFilter_Wid = 10; smooth_res = zeros(1,N);kalman_p = zeros(1,N); for

18、i = Filter_Wid + 1 : Ntempsum = 0; kalman_m = 0;for j = i - Filter_Wid : i - 1tempsum = tempsum + y(j); kalman_m = kalman_m+x(j);end平滑滤波kalman_p(i) = kalman_m/Filter_Wid; smooth_res(i) = tempsum / Filter_Wid;%end % figure(1);% hist(y);t=1:N;figure(1);expValue = zeros(1,N);for i = 1: NexpValue(i) = CON;endplot(t,expValue,'r',t,x,'g',t,y,'b',