第三章卡尔曼滤波仿真作业-第2题

仿真原理给定条件设有一个随机信号服从AR(4)过程，它是一个宽带过程，参数如下：a(1)a(2)a(3)a(4)-1.3521.338-0.6620.2401通过观测方程来测量信号，是方差为1的高斯白噪声2、推导初始条件如下：由所给定的条件，得到状态方程如下：x(n)=1.352*x(n-1)-1.338*x(n-2)+0.662*x(n-3)-0.24*x(n-4)+w(n)其中w(n)表示动态噪声，其方差为1；令x(-1)=x(-2)=x(-3)=x(-4)=0得到的初始值如下：x(1)=w(n)；x(2)=w(n)+1.352*x(1)；x(3)=w(n)+1.352*x(2)-1.338*x(1)；x(4)=w(n)+1.352*x(3)-1.338*x(2)+0.602*x(1)；当n≥5时满足如下方程：x(n)=1.352*x(n-1)-1.338*x(n-2)+0.602*x(n-3)-0.24*x(n-4)+w(n);其中是方差为1的高斯白噪声对卡尔曼滤波有：由状态方程可得状态变量增益矩阵为：A=仅对x方向进行估计，即考虑一维的情况下，有：状态变量与输出信号之间的增益矩阵Ck=[1000];量测噪声协方差阵为Rk=[1];噪声的均方误差阵为Pk=；3、仿真要求分别利用Wiener滤波器和Kalman滤波器通过测量信号估计的波形4、仿真原理（1）维纳滤波原理维纳滤波的原理是根据全部过去的和当前的观测数据x(n),x(n-1),…来估计信号的当前值。以均方误差最小条件下求解系统的传递函数H(z)或单位冲激响应h(n)。维纳滤波的信号模型是从信号与噪声的相关函数得到。Wiener滤波器的一般结构如下：线性滤波器线性滤波器有一期望响应d(n),滤波器系数的设计准则是使得滤波器的输出y(n)是均方意义上对期望响应的最优线性估计。线性系统输出为：均方误差为：维纳滤波器的设计，实际上就是在最小均方误差条件下，即，确定滤波器的冲激响应h(n)或系统函数H(z)，可等效于求解维纳-霍夫方程：[h]=[φxx]-1[φxs]。（2）卡尔曼滤波原理不需要全部过去的观察数据,只根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态空间法描述系统，即由状态方程和量测方程组成。解是以估计值(是状态变量的估计值)的形式给出的，其算法是递推。卡尔曼滤波的信号模型则是从状态方程和量测方程得到。卡尔曼滤波的信号模型（一维）如下：离散系统的n维状态方程：离散系统的m维量测方程：令表示状态变量之间的增益矩阵，为状态变量与输出信号之间的增益矩阵，不随时间发生变化，动态噪声与观测噪声都是零均值的正态噪声，且两者互不相关，为量测噪声协方差矩阵，为动态噪声协方差矩阵。系统初始条件为：卡尔曼滤波的基本思想是先不考虑激励噪声和观测噪声，得到状态的估计值和观测数据的估计值,再用观测数据的估计误差去修正状态的估计值，通过选择修正矩阵H使得状态估计误差的均方值最小。卡尔曼滤波的递推公式如下：假设初始条件已知，其中，则卡尔曼滤波的递推流程如下：仿真流程(1)维纳滤波仿真流程如下：初始条件+状态方程初始条件+状态方程真实值x观测样本y=x+v，v表示加性高斯白噪声观测信号的自相关函数φyy观测信号与期望信号的互相关函数φyx维纳滤波器系数：[h]=[φyy]-1[φyx]输出滤波估计值xk_s误差=真实值-滤波估计值

_(2)卡尔曼滤波仿真流程如下：给定A给定Ak、Ck、Rk、Pk的初始值计算未考虑噪声时的均方误差阵：计算增益矩阵：计算估计值：计算滤波后的均方误差阵：k=k+1k=k+1真实值x误差=真实值-滤波估计值观测值ykyk=x+v

_三、仿真结果及分析1、利用维纳滤波器进行估计：(1)Wiener滤波器的阶数取101阶，观测点数取100，的方差为1时，通过仿真得到真实轨迹、观测样本及估计轨迹的比较图形如下图1所示：图1采用维纳滤波，噪声方差为1时，真实轨迹、观测样本及估计轨迹的比较从图1可以看出，利用Wiener滤波器通过测量信号估计的波形，得到的估计轨迹接近于真实轨迹。当噪声方差为1时，估计轨迹与真实轨迹间的误差很小。(2)Wiener滤波器的阶数取101阶，观测点数取100，的方差为1时，通过仿真得到平均误差如下图2所示：图2采用维纳滤波，噪声方差为1时的平均误差从图2可以看出，利用Wiener滤波器通过测量信号估计的波形，当噪声方差为1时，得到的估计轨迹与真实轨迹的平均误差较小，而且刚开始的时候平均误差相对较大，随着时间的推移，平均误差有逐渐减小的趋势。(3)Wiener滤波器的阶数取101阶，观测点数取100，的方差为4时，通过仿真得到真实轨迹、观测样本及估计轨迹的比较图形如下图3所示：图3采用维纳滤波，噪声方差为4时，真实轨迹、观测样本及估计轨迹的比较从图3可以看出，当噪声方差为4时，估计轨迹与真实轨迹间的误差变大了。利用Wiener滤波器通过测量信号估计的波形，得到的估计轨迹接近于真实轨迹。(4)Wiener滤波器的阶数取101阶，观测点数取100，的方差为4时，通过仿真得到平均误差如下图4所示：图4采用维纳滤波，噪声方差为4时的平均误差从图4可以看出，当噪声方差为4时，估计轨迹与真实轨迹间的平均误差变大了。而且刚开始的时候平均误差相对较大，随着时间的推移，平均误差有逐渐减小的趋势。2、利用卡尔曼滤波器进行估计：(1)采用卡尔曼滤波，观测点数取100，的方差为1时，通过仿真得到真实轨迹、观测样本及估计轨迹的比较图形如下图5所示：图5采用卡尔曼滤波，噪声方差为1时，真实轨迹、观测样本及估计轨迹的比较从图5可以看出，利用卡尔曼滤波器，通过测量信号估计的波形，得到的估计轨迹接近于真实轨迹。当噪声方差为1时，估计轨迹与真实轨迹间的误差很小。(2)采用卡尔曼滤波，观测点数取100，的方差为1时，通过仿真得到平均误差如下图6所示：图6采用卡尔曼滤波，噪声方差为1时的平均误差从图6可以看出，利用卡尔曼滤波器通过测量信号估计的波形，当噪声方差为1时，得到的估计轨迹与真实轨迹的平均误差较小。(3)采用卡尔曼滤波，观测点数取100，的方差为4时，通过仿真得到真实轨迹、观测样本及估计轨迹的比较图形如下图7所示：图7采用卡尔曼滤波，噪声方差为4时，真实轨迹、观测样本及估计轨迹的比较从图7可以看出，当噪声方差为4时，估计轨迹与真实轨迹间的误差变大了。利用卡尔曼滤波器通过测量信号估计的波形，得到的估计轨迹接近于真实轨迹。(4)采用卡尔曼滤波，观测点数取100，的方差为4时，通过仿真得到平均误差如下图8所示：图8采用卡尔曼滤波，噪声方差为4时的平均误差从图8可以看出，利用卡尔曼滤波器通过测量信号估计的波形，当噪声方差为4时，估计轨迹与真实轨迹间的平均误差变大了。四、结论通过以上的仿真结果及其分析可以得到如下结论：（1）利用Wiener滤波器和Kalman滤波器都可以通过测量信号对的波形进行估计，估计轨迹接近于真实轨迹；（2）随着噪声方差的变大，通过维纳滤波和卡尔曼滤波得到的估计轨迹与真实轨迹间的误差也随之变大；（3）相比较而言，利用维纳滤波器进行估计的误差稍小于通过卡尔曼滤波器进行估计的误差；（4）在利用维纳滤波器和卡尔曼滤波器进行估计时，刚开始的时候平均误差相对较大，随着时间的推移，平均误差有逐渐减小的趋势。五、附件：维纳滤波仿真源程序（噪声方差取1）clc;clearall;maxlag=100;%%%%%%%%%%%%%%%%%%%%%%%%N=100;%观测点数取100x=zeros(N,1);y=zeros(N,1);var=1;%%%%%%%%%%%%%%%%%%%%%%%%%列出状态方程x(1)=randn(1,1);%令x(-1)=x(-2)=x(-3)=x(-4)=0x(2)=randn(1,1)+1.352*x(1);x(3)=randn(1,1)+1.352*x(2)-1.338*x(1);x(4)=randn(1,1)+1.352*x(3)-1.338*x(2)+0.602*x(1);forn=5:Nx(n)=1.352*x(n-1)-1.338*x(n-2)+0.602*x(n-3)-0.24*x(n-4)+randn(1,1);%x为真实值end;v=randn(N,1);y=x+v;%z_x为观测样本值=真值+噪声%%%%%%%%%%%%%%%%%%%%%%%%滤波x=x';y=y';xk_s(1)=y(1);%赋初值xk_s(2)=y(2);xk_s(3)=y(3);xk_s(4)=y(4);xk=[y(1);y(2);y(3);y(4)];%%%%%%%%%%%%%%%%%%%%%%%%%维纳滤波器的生成[rx,lags]=xcorr(y,maxlag,'biased');%观测信号的自相关函数rx1=toeplitz(rx(101:end));%对称化自相关函数矩阵使之成为方阵，滤波器的阶数为101阶rx2=xcorr(x,y,maxlag,'biased');%观测信号与期望信号的互相关函数rx2=rx2(101:end);h=inv(rx1)*rx2';%维纳-霍夫方程xk_s=filter(h,1,y);%加噪信号通过滤波器后的输出%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%e_x=0;eq_x=0;e_x1=N:1;%计算滤波的均值,计算滤波误差的均值fori=1:Ne_x(i)=x(i)-xk_s(i);%误差=真实值-滤波估计值end%%%%%%%%%%%%%%%%%%%%%%%作图t=1:N;figure(1);plot(t,x,'r-',t,y,'g:',t,xk_s,'b-.');legend('真实轨迹','观测样本','估计轨迹');figure(2);plot(e_x);legend('平均误差');维纳滤波仿真源程序（噪声方差取4）clc;clearall;maxlag=100;%%%%%%%%%%%%%%%%%%%%%%%%N=100;%观测点数取100x=zeros(N,1);y=zeros(N,1);var=2;%%%%%%%%%%%%%%%%%%%%%%%%%列出状态方程x(1)=randn(1,1);%令x(-1)=x(-2)=x(-3)=x(-4)=0x(2)=randn(1,1)+1.352*x(1);x(3)=randn(1,1)+1.352*x(2)-1.338*x(1);x(4)=randn(1,1)+1.352*x(3)-1.338*x(2)+0.602*x(1);forn=5:Nx(n)=1.352*x(n-1)-1.338*x(n-2)+0.602*x(n-3)-0.24*x(n-4)+randn(1,1);%x为真实值end;v=4*randn(N,1);y=x+v;%z_x为观测样本值=真值+噪声%%%%%%%%%%%%%%%%%%%%%%%%滤波x=x';y=y';xk_s(1)=y(1);%赋初值xk_s(2)=y(2);xk_s(3)=y(3);xk_s(4)=y(4);xk=[y(1);y(2);y(3);y(4)];%%%%%%%%%%%%%%%%%%%%%%%%%维纳滤波器的生成[rx,lags]=xcorr(y,maxlag,'biased');%观测信号的自相关函数rx1=toeplitz(rx(101:end));%对称化自相关函数矩阵使之成为方阵，滤波器的阶数为101阶rx2=xcorr(x,y,maxlag,'biased');%观测信号与期望信号的互相关函数rx2=rx2(101:end);h=inv(rx1)*rx2';%维纳-霍夫方程xk_s=filter(h,1,y);%加噪信号通过滤波器后的输出%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%e_x=0;eq_x=0;e_x1=N:1;%计算滤波的均值,计算滤波误差的均值fori=1:Ne_x(i)=x(i)-xk_s(i);%误差=真实值-滤波估计值end%%%%%%%%%%%%%%%%%%%%%%%作图t=1:N;figure(1);plot(t,x,'r-',t,y,'g:',t,xk_s,'b-.');legend('真实轨迹','观测样本','估计轨迹');figure(2);plot(e_x);legend('平均误差');卡尔曼滤波仿真源程序（噪声方差取1）clc;clear;%%%%%%%%%%%%%%%%%%%%%%%%N=100;%观测点数取100x=zeros(N,1);y=zeros(N,1);var=1;I=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];%I为四阶单位阵%%%%%%%%%%%%%%%%%%%%%%%%%列出状态方程x(1)=randn(1,1);%令x(-1)=x(-2)=x(-3)=x(-4)=0x(2)=randn(1,1)+1.352*x(1);x(3)=randn(1,1)+1.352*x(2)-1.338*x(1);x(4)=randn(1,1)+1.352*x(3)-1.338*x(2)+0.602*x(1);forn=5:Nx(n)=1.352*x(n-1)-1.338*x(n-2)+0.602*x(n-3)-0.24*x(n-4)+randn(1,1);%x为真实值end;v=randn(N,1);y=x+v;%z_x为观测样本值=真值+噪声%%%%%%%%%%%%%%%%%%%%%%%%滤波xk_s(1)=y(1);%赋初值xk_s(2)=y(2);xk_s(3)=y(3);xk_s(4)=y(4);Ak=[1.352,-1.338,0.662,0.240;1,0,0,0;0,1,0,0;0,0,1,0];%状态变量之间的增益矩阵AkCk=[1000];%状态变量与输出信号之间的增益矩阵Ck,(一维:仅仅对x方向进行估计)Rk=[1];%量测噪声协方差阵Pk=[1000010000100001];%噪声的均方误差阵xk=[y(1);y(2);y(3);y(4)];Qk=[1];%%%%%%%%%%%%%%%%%%%%%%%%Kalman滤波开始,估计循环forr=5:Nyk=y(r);Pk1=Ak*Pk*Ak'+Qk;%(未考虑噪声)k时刻滤波的均方误差矩阵Hk=Pk1*Ck'*inv(Ck*Pk1*Ck'+Rk);%增益方程xk=Ak*xk+Hk*(yk-Ck*Ak*xk);%递推公式Pk=(I-Hk*Ck)*Pk1;%滤波后的均方误差矩阵xk_s(r)=xk(1,1);%xk_s为估计值end%ende_x=0;%计算滤波的均值,计算滤波误差的均值fori=1:Ne_x(i)=x(i)-xk_s(i);%误差=真实值-滤波估计值end%%%%%%%%%%%%%%%%%%%%%%%作图t=1:N;figure(1);plot(t,x,'r-',t,y,'g:',t,xk_s,'b-.');legend('真实轨迹','观测样本','估计轨迹');figure(2);plot(e_x);legend('平均误差');卡尔曼滤波仿真源程序（噪声方差取4）clc;clear;%%%%%%%%%%%%%%%%%%%%%%%%N=100;%观测点数取100x=zeros(N,1);y=zeros(N,1);var=2;I=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];%I为四阶单位阵%%%%%%%%%%%%%%%%%%%%%%%%%列出状态方程x(1)=randn(1,1);%令x(-1)=x(-2)=x(-3)=x(-4)=0x(2)=randn(1,1)+1.352*x(1);x(3)=randn(1,1)+1.352*x(2)-1.338*x(1);x(4)=randn(1,1)+1.352*x(3)-1.338*x(2)+0.602*x(1);forn=5:Nx(n)=1.352*x(n-1)-1.338*x(n-2)+0.602*x(n-3)-0.24*x(n-4)+randn(1,1);%x为真实值end;v=4*randn(N,1);y=x+v;%z_x为观测样本值=真值+噪声%%%%%%%%%%%%%%%%%