卡尔曼滤波教程.docVIP

卡尔曼滤波报告 实验任务 产生含噪声信号X(n)=sin(2*pi*f*n)+w(n),f=0.05,w(n)~N(0,1.2)。编写程序运用卡尔曼滤波进行去噪处理，要求画出去噪前和去噪后图形，滤波误差及收敛过程。 二、实验程序 %***离散线性时不变系统的状态空间模型为： %***X(k+1)=A*X(k)+B*U(k)+w(k) %***Z(k)=H*X(k)+v(k) 每一个时刻都有噪声加入，算法是实时修正上一时刻的状态值 clc; clear; %%产生有用信号s(n),加噪信号x(n) N=256 ; %信号与噪声的长度 离散信号个数 w=randn(1.2,N); %产生高斯白噪声，令方差为1.2 f=0.05; %实正弦信号频率 s=sin(2*pi*f*(0:N-1)) ; %产生正弦信号 subplot(311); plot(s); title(有用信号s(n)) grid on; x=s+w; subplot(312); plot(x); title(加噪信号x(n)) grid on; c=1; %观测矩阵 a=[1]; %状态转移矩阵 b=[1]; %输入矩阵 H=[1]; R=std(w); %R是观测白噪声v(k)的方差 %% ***卡尔曼滤波循环 Y(1)=20; P(1)=10; for i=1:1:N-1 Y(i+1)=a*Y(i)+b*s(i); P(i+1)=a*P(i); Kg(i)=P(i+1)*H*inv(H*P(i+1)*H+R); Y(i+1)= Y(i+1)+Kg(i)*(x(i)-H*Y(i+1)); P(i+1)=P(i+1)-Kg(i)*H*P(i+1); end; subplot(313); t=1:N; plot(t,Y); title(通过卡尔曼滤波后的估计信号y(n)) grid on; 实验结果 Kalman滤波器的效果是使输出变得更平滑，但没办法去除信号中原有的椒盐噪声，而且，Kalman滤波器也会跟踪这些椒盐噪声点，因此推荐在使用Kalman滤波器前先使用中值滤波去除椒盐噪声。 实验总结 卡尔曼滤波器是建立在动态过程之上，由于物理量（位移，速度）的不可突变特性，这样就可以通过t-1时刻估计（预测）t时刻的状态，其状态空间模型为： 其中各个量之间的含义是： x(n)是状态向量，包含了观测的目标 u(n)是驱动输入向量，A是状态转移矩阵，其隐含指示了“n-1时刻的状态会影响到n时刻的状态（这似乎和马尔可夫过程有些类似）” B是控制输入矩阵，其隐含指示了“n时刻给的驱动如何影响n时刻的状态” 在我看来物理学中的匀加速直线运动过程就是卡尔曼滤波中状态空间模型的一个典型应用。 从运动的角度，很容易理解：小车当前n时刻的位移和速度一部分来自于n-1时刻的惯性作用，这通过Ax(n)来度量，另一部分来自于现在n时刻小车新增加的外部受力，通过Bu(n)来度量。 w(n)是过程噪声，w(n)~N(0,Q)的高斯分布，过程噪声是使用卡尔曼滤波器时一个重要的量。 计算n时刻的位移，还有一种方法：拿一把长的卷尺（嗯，如果小车跑了很长时间，估计这把卷尺就难买到了），从起点一拉，直接就出来了，设测量值为z(n)。计算速度呢？速度传感器往那一用就出来了。 然而，初中物理就告诉我们，“尺子是量不准的，物体的物理真实值无法获得”，测量存在误差，我们暂且将这个误差记为v(n)。这种通过直接测量的方式获得所需物理量的值构成观测空间： z(n)就是测量结果，H(n)是观测矢量，x(n)就是要求的物理量（位移、速度），v(n)~N(0,R)为测量噪声. 现在就有了两种方法可以得到n时刻的位移和速度：一种就是通过状态空间递推计算，另一种就是直接拿尺子和传感器测量。致命的是没一个是精确无误的,分别都存在0均值高斯分布的误差w(n)和v(n). 为充分利用测量值和预测值，Kalman滤波并不是简单的取其中一个作为输出，也不是求平均。 设预测过程噪声w(n)~N(0,Q)，测量噪声v(n)~N(0,R)。Kalman计算输出分为预测过程和修正过程如下： 预测 预测值： 最小均方误差矩阵： 修正 误差增益：卡尔曼增益K的含义就是估计量的方差占总方差（包括估计方差和测量方差）的比重。 修正值： 最小均方误差矩阵： 以上各式中： x(n)：Nx1的状态矢量 z(n)：Mx1的观测矢量，Kalman滤波器的输入 x(n|n-1)：用n时刻以前的数据进行对n时刻