卡尔曼滤波器及matlab代码

信息融合大作业——维纳最速下降法滤波器，卡尔曼滤波器设计及 Matlab 仿真时间：2010-12-6 专业：信息工程 班级：09030702 学号：2007302171 姓名：马志强1 / 181. 滤波问题浅谈估计器或滤波器这一术语通常用来称呼一个系统，设计这样的系统是为了从 含有噪声的数据中提取人们感兴趣的，接近规定质量的信息。

由于这样一个宽 目标，估计理论应用于诸如通信、雷达、声纳、导航、地震学、生物医学工程、 金融工程等众多不同的领域。

例如，考虑一个数字通信系统，其基本形式由发 射机、信道和接收机连接组成。

发射机的作用是把数字源(例如计算机)产生的 0、 1 符号序列组成的消息信号变换成为适合于信道上传送的波形。

而由于符号间 干扰和噪声的存在，信道输出端收到的信号是含有噪声的或失真的发送信号。

接收机的作用是，操作接收信号并把原消息信号的一个可靠估值传递给系统输 出端的某个用户。

随着通信系统复杂度的提高，对原消息信号的还原成为通信 系统中最为重要的环节，而噪声是接收端需要排除的最主要的干扰，人们也设 计出了针对各种不同条件应用的滤波器，其中最速下降算法是一种古老的最优 化技术，而卡尔曼滤波器随着应用条件的精简成为了普适性的高效滤波器。

2．维纳最速下降算法滤波器2.1 最速下降算法的基本思想考虑一个代价函数 ，它是某个未知向量 的连续可微分函数。

函数将 的元素映射为实数。

这里，我们要寻找一个最优解 。

使它满足如下条件(2.1) 这也是无约束最优化的数学表示。

特别适合于自适应滤波的一类无约束最优化算法基于局部迭代下降的算 法：从某一初始猜想 出发，产生一系列权向量，使得代价函数在算法的每一次迭代都是下降的，即其中是权向量的过去值，而是其更新值。

2 / 18我们希望算法最终收敛到最优值 。

迭代下降的一种简单形式是最速下降 法，该方法是沿最速下降方向连续调整权向量。

为方便起见，我们将梯度向量 表示为(2.2) 因此，最速下降法可以表示为(2.3) 其中 代表进程， 是正常数，称为步长参数，1/2 因子的引入是为了数学上处 理方便。

在从 到 的迭代中，权向量的调整量为为了证明最速下降算法满足式(2.1)，在(2.4) 处进行一阶泰勒展开，得到(2.5) 此式对于 较小时是成立的。

在式(2.4)中设 为负值向量，因而梯度向量 也为 负值向量，所以使用埃尔米特转置。

将式(2.4)用到式(2.5)中，得到此式表明当 为正数时，。

因此，随着 的增加，代价函数 减小，当时，代价函数趋于最小值 。

2.2 最速下降算法应用于维纳滤波器3 / 18考虑一个横向滤波器，其抽头输入为，对应的抽头权值为。

抽头输入是来自零均值、相关矩阵为 的广义平稳随机过程的抽样值。

除了这些输入外，滤波器还要一个期望响应 ，以便为最优滤波提供一个参考。

在时刻 抽头输入向量表示为 ，滤波器输出端期望响应的估计值为,其中 是由抽头输所张成的空间。

空过比较期望响应值，可以得到一个估计误差 ，即及其估计这里 一步表示为是抽头权向量与抽头输入向量 的内积。

(2.6) 可以进同样，抽头输入向量 可表示为如果抽头输入向量 和期望响应 是联合平稳的，此时均方误差或者 在时刻 的代价函数 是抽头权向量的二次函数，于是可以得到(2.7) 其中， 为目标函数 的方差， 抽头输入向量 与期望响应 的互相 关向量，及 为抽头输入向量 的相关矩阵。

从而梯度向量可以写为4 / 18(2.8)其中在列向量中和分别是代价函数 对应第 个抽头权值的实部 和虚部 的偏导数。

对最速下降算法应用而言，假设式(2.8)中相关矩阵 和互相关向量 已知，则对于给定的抽头权向量为(2.9) 它描述了为那滤波中最速下降法的数学表达式。

3.卡尔曼滤波器3.1 卡尔曼滤波器的基本思想卡尔曼滤波器是用状态空间概念描述其数学公式的，另外新颖的特点是， 他的解递归运算，可以不加修改地应用于平稳和非平稳环境。

尤其是，其状态 的每一次更新估计都由前一次估计和新的输入数据计算得到，因此只需存储前 一次估计。

除了不需要存储过去的所有观测数据外，卡尔曼滤波计算比直接根 据滤波过程中每一步所有过去数据进行估值的方法都更加有效。

++5 / 18图 3.1 线性动态离散时间系统的信号流图表示“状态”的概念是这种表示的基础。

状态向量，简单地说状态，定义为数据 的最小集合，这组数据足以唯一地描述系统的自然动态行为。

换句话说，状态由 预测系统未来特性时所素要的，与系统的过去行为有关的最少的数据组成。

典型地，比较有代表性的情况是，状态 是未知的。

为了估计它，我们使用一组观测数据，在途中用向量 表示。

成为观测向量或者简称观测值，并假设它是 维的。

在数学上，图 3.1 表示的信号流图隐含着一下两个方程：(1)过程方程式中，向量其相关矩阵定义为(3.1) 表示噪声过程，可建模为零均值的白噪声过程，且(2)测量方程其中 是已知的测量矩阵。

向量模为零均值的白噪声过程，其相关矩阵为(3.2) 称为测量噪声，建测量方程(3.2)确立了可测系统输出 3.1 所示。

3.2 新息过程6 / 18与状态(3.3) 之间的关系，如图为了求解卡尔曼滤波问题，我们将应用基于新息过程的方法。

根据之前所述，用向量表示 时刻到 时刻所有观测数据过去值给定的情况下，你时刻观测数据 的最小均方估计。

过去的值用观测值息过程如下：表示，他们张成的向量空间用 表示。

从而可以定义新(3.4)其中向量 表示观测数据 的新息。

3.3 应用新息过程进行状态估计下面，我们根据信息过程导出状态 的最小均方估计。

根据推导，这个估计可以表示成为新息过程序列的线性组合，即其中是一组待定的与新息过程正交，即(3.5) 矩阵。

根据正交性原理，预测状态误差向量(3.6) 将式(3.5)代入式(3.6)，并利用新息过程的正交性质，即得因此，式(3.7)两边同时右乘逆矩阵，可得(3.7) 的表达式为(3.8) 最后，将式(3.8)带入式(3.5)，可得最小军方差估计7 / 18(3.9)故对于，有然而， 时刻的状态于，有与 时刻的状态(3.10) 的关系式由式可以推导出对其中 只与观测数据有关。

因此可知，(3.11) 与 彼此正交(其中)。

利用式(3.11)以及当 时右边的求和项改写为的计算公式，可将式(3.10)为了进一步讨论，引入如下基本定义。

3.4 卡尔曼增益定义矩阵8 / 18(3.12)其中是状态向量和新息过程这一定义和式(3.12)的结果，可以将式(3.10)简单重写为(3.13) 的互相关矩阵。

利用(3.14) 式(3.14)具有明确的物理意义。

它标明：线性动态系统状态的最小均方估计可以由前一个估计求得。

为了表示对卡尔曼开创性贡献的认可，将矩阵 称为卡尔曼增益。

现在剩下唯一要解决的问题是，怎样以一种便于计算的形式来表示卡尔曼增益 。

为此，首先将与乘积的期望表示为式中利用了状态 与噪声向量(3.15) 互不相关这一事实。

其次，由于预测状态误差向量与估计正交，因此与期望为零。

这样，用预测状态误差向量 起式(3.15)变化，故有代替相乘因子乘机的 ，将不会引由此，可将上式进一步变化为(3.16)现在我们重新定义卡尔曼增益。

为此，将式(3.17)代入式(3.13)得(3.17)(3.18) 现在我们已经了解了卡尔曼滤波的整个过程和相应的参数设置，为了能够更为方9 / 18便利用计算机仿真实现，特将其中参数变量进行小结。

卡尔曼变量和参数小结变量定义时刻状态时刻状态值从 时刻到 时刻的转移矩阵时刻的测量矩阵过程噪声的相关矩阵过程噪声的相关矩阵给定观测值在 时刻状态的预测估计给定观测值在时刻状态的滤波估计时刻卡尔曼增益矩阵 时刻新息向量 新息向量 的相关矩阵中误差相关矩阵中误差相关矩阵维数观测值= 转移矩阵=基于单步预测的卡尔曼滤波器的小结10 / 18测量矩阵=过程噪声的相关矩阵=测量噪声的相关矩阵=4 Matlab仿真为了简化，这里只讨论简单的一维单输入—单输出线性系统模型，其中加入白噪声作为系统的扰动，具体仿真结果可以获得如下4.1 维纳最速下降法滤波器仿真结果以上为最速下降法中不同的递归步长所导致的跟踪效果变化，对于最速下降法中的步长是影响其算法稳定的关键，最速下降算法稳定的充分必要条件是条件步长因子为小于输入自相关矩阵的最大特征值倒数的2倍。

上面的序列分别从相关矩阵的随大特征之2倍的0.4倍开始变化至其1倍，最后一幅图象能够看出其已经不再收敛，下面是大于输入相关矩阵的最大特征值2倍步长时所表现的跟踪结果可以看出其已经明显发散，不再是我们所期望的滤波算法。

因此可以总结出，对于最速下降法来说，步长的选取是很重要的，根据不同条件的需求，选取正确的步长，能为算法的快速高效提供基础。

4.2 卡尔曼滤波器仿真结果从图中可以发现，卡尔曼滤波器能够非常有效地在比较大的干扰下比较准确地反映真实值，如果观测端加入干扰较大时，卡尔曼滤波器能够较为有效地进行滤波，不过当状态端的干扰增大时，卡尔曼滤波器的滤波效果也会随之下降。

如下图，是加大了状态端的干扰，所呈现的滤波效果。

如上图所示，状态端的干扰导致状态不稳定，卡尔曼滤波器的估计值也出现了比较大的波动。

如果将状态端的干扰再增大，则会出现更为严峻的滤波考验，滤波效果如下。

这是的状态已经很勉强了，所以，研究更为有效的多方法卡尔曼滤波器也显得十分必要了。

4.3 一种不需初始化的卡尔曼滤波器仿真这种滤波器只是实现了无需对部分变量进行初始化的设计，没有特别意义上的改进经典卡尔曼滤波器本身性能的特点。

仿真图如下4.4 后联平滑滤波的卡尔曼滤波器仿真只是在经典卡尔曼滤波器后端联接了平滑滤波器，对性能改进的效果并不特别明显，仿真图如下如图中所表示，即使平滑过的估值与观测值之间的差别也不是特别令人满意，所以，对于经典卡尔曼滤波的研究还需要更深一步进行，由于时间和能力有限，本次的作业对于卡尔曼及其他滤波器的研究只能达到这种程度，希望在以后的学习中，能发现更好的对经典卡尔曼滤波器的改进方法。

5 Matlab源代码(部分参考自互联网)5.1经典卡尔曼滤波器clearN=200;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(t)-c*b(t)*p1(t);endt=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');%红色卡尔曼，绿色观测值，蓝色状态值legend('kalman estimate','ovservations','truth');5.2 最速下降法clcclear allN=30;q=2.1;%q>1&&q<2/Ryx最大特征值hn=zeros(1,N);hn(:)=5;vg=0;Rxx=xcorr(1);Ryx=min(min(corrcoef(1, 1+randn)));echo offfor 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;endt=1:N-1;plot(t,hn(t),'r-',t,m(t),'b-');5.3 后联平滑滤波器的卡尔曼滤波器clearclc;N=300;CON = 5;x = zeros(1,N);x(1) = 1;p = 10;Q = randn(1,N)*0.2;%过程噪声协方差R = randn(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 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);endkalman_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',t,smooth_res,'k',t,kalman_p,'m'); legend('truth','estimate','measure','smooth result','smooth kalman'); %axis([0 N 0 30])xlabel('Sample time');ylabel('Room Temperature');title('Smooth filter VS kalman filter');。