卡尔曼滤波报告

卡尔曼滤波报告一、实验任务产生含噪声信号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.2f=0.05;%实正弦信号频率s=sin(2*pi*f*(0:N-1));%产生正弦信号subplot(311);plot(s);title('有用信号s(n)')gridon;x=s+w;subplot(312);plot(x);title('加噪信号x(n)')gridon;c=1;%观测矩阵a=[1];%状态转移矩阵b=[1];%输入矩阵H=[1];R=std(w);%R是观测白噪声v(k)的方差%%***卡尔曼滤波循环Y(1)=20;P(1)=10;fori=1:1:N-1Y(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)')gridon;三、实验结果Kalman滤波器的效果是使输出变得更平滑，但没办法去除信号中原有的椒盐噪声，而且，Kalman滤波器也会跟踪这些椒盐噪声点，因此推荐在使用Kalman滤波器前先使用中值滤波去除椒盐噪声。四、实验总结卡尔曼滤波器是建立在动态过程之上，由于物理量（位移，速度）的不可突变特性，这样就可以通过t-1时刻估计（预测）t时刻的状态，其状态空间模型为：nwnBunAxn1x其中各个量之间的含义是：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)。这种通过直接测量的方式获得所需物理量的值构成观测空间：nvnxnHnzz(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计算输出分为预测过程和修正过程如下：预测预测值：nBunnxAnn1|11|x最小均方误差矩阵：QnnAPnPAT1|11|n修正误差增益：卡尔曼增益K的含义就是估计量的方差占总方差（包括估计方差和测量方差）的比重。nHnnPnHnRHTnnnPKT1|11|n修正值：1|1||xnnxnHnznKnnxAnn最小均方误差矩阵：1||nnnPnHnKInP以上各式中：x(n)：Nx1的状态矢量z(n)：Mx1的观测矢量，Kalman滤波器的输入x(n|n-1)：用n时刻以前的数据进行对n时刻的估计结果x(n|n)：用n时刻及n时刻以前的数据对n时刻的估计结果，这也是Kalman滤波器的输出P(n|n-1)：NxN，最小预测均方误差矩阵，其定义式为1|1|1|nnnxnxTnnxnxEnPP(n|n)：NxN，修正后最小均方误差矩阵。K(n)：NxM，误差增益，从增益的表达式看，相当于“预测最小均方误差”除以“n时刻的测量误差+预测最小均方误差”，直观含义就是用n-1预测n时刻状态的预测最小均方误差在n时刻的总误差中的比重，比重越大，说明真值接近预测值的概率越小（接近测量值的概率越大）。综上，借用一张图说明一下Kalman滤波算法的流程图中的符号和本文符号稍有差异，主要是P的表示上。从上图也可以看出，Kalman滤波就是给定-1时刻的初始值，然后在预测（状态空间）和修正（观测空间）之间不停的递推，求取n时刻的估计x和均方误差矩阵P。Kalman滤波器就是充分结合了估计值和测量值得到n时刻更接近真值的估计结果。Kalman滤波器巧妙的用“独立高斯分布的乘积”将这两个测量值和估计值进行融合！