卡尔曼滤波

卡尔曼滤波简介一.背景简介二.Kalman滤波理论基础介绍及应用三.Kalman滤波在自由落体运动目标跟踪中的应用四.MATLAB仿真程序及结果目录Contents匈牙利数学家1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。1960年发表的论文《ANewApproachtoLinearFilteringandPredictionProblems》（线性滤波与预测问题的新方法）背景简介滤波理论是对系统可观测信号进行测量的基础，根据滤波准则，采用某种统计量最优方法，对系统的状态进行估计的理论和方法。最优滤波或最优估计：在最小方差意义下的最优滤波或估计，即要求信号或状态的最优估值应与相应的真实值的误差的方差最小。经典最优滤波理论包括Wiener(维纳）滤波理论和Kalman滤波理论。Kalman滤波采用的是时域状态空间方法，实时递推算法。背景简介在工程控制系统和信息处理问题中，通常所得到的观测信号中不仅包含所需信号，而且包含有随机观测噪声和干扰信号。通过对一系列带有观测噪声和干扰信号的实际观测数据处理，从中得到所需的各种参量的估计值。在工程实践中，估计问题两种：1.系统的结构参数部分或全部未知，有待确认。2.实施最优控制需要随时了解系统的状态，而由于种种限制，系统中的一部分或全部状态变量不能直接测得，这就形成了估计的两类问题——参数估计和状态估计。Kalman滤波理论基础与应用只要与时间序列和高斯白噪声有关或者能建立类似模型的系统，都可以用Kalman滤波来处理噪声问题，用来预测，滤波。Kalman滤波应用：（1）导航制导，无线定位和目标跟踪。（2）通信与信号处理，图像复原，语音增强。（3）天气预报，地震预报。（4）地质勘探，矿物开采。(5)故障诊断，检测，证券股票市场预测。Kalman滤波理论基础与应用假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（WhiteGaussianNoise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（GaussianDistribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。卡尔曼滤波理论介绍假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。卡尔曼滤波理论介绍究竟实际温度是多少呢？我们可以用他们的covariance（协方差）来判断。因为Kg=5^2/(5^2+4^2)，所以Kg=0.6098，我们可以估算出k时刻的实际温度值是：23+0.6098*(25-23)=24.22度。可以看出，因为温度计的covariance比较小，所以估算出的最优温度值偏向温度计的值。现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。((1-Kg)*5^2)^0.5=3.12。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。卡尔曼滤波理论介绍卡尔曼滤波理论介绍1.随机线性离散系统模型式中的是系统状态向量，是系统的观测向量，是随机干扰向量，是系统的观测噪声向量，是系统状态转移矩阵，是干扰输入矩阵，是观测矩阵。Kalman滤波理论基础与应用,11,11kkkkkkkXXWkkkkZHXVkXkZkWkV,1kk,1kkkH某一物体在重力场做自由落体运动，观测装置对其位移进行检测，在传感器受到未知的独立分布随机信号的干扰下，我们需要估计该物体的运动位移和速度。Kalman滤波在自由落体运动目标跟踪中的应用状态方程的建立：由已有的运动学方程，得到该运动物体的状态方程给定位置观测装置，其观测方程为：给定v(k)的方差:定义均方误差:估计器为：Kalman滤波在自由落体运动目标跟踪中的应用110.5x()(1)()011kxkg(k)(1)()[y(k)CA(1)]xAxkKkxk()()()Ykxkvk2()1Rk2()[()]PkEek其中递推关系式为：估计器表达式，把它分成两部分，前者为预测，后者为修正：第k时刻的估计是由k-1时刻的预测值加上修正量得到的，k+1时刻的预测值：Kalman预测器为:Kalman滤波在自由落体运动目标跟踪中的应用1()(1)(1)TPkAPkAQk111()()[()]TTKkPkCCPkCR11()()()()PkPkKkCPk()(1)K(k)[y(k)CA(1)]xkAxkxk(1/)()xkkAxk(1/)()A{A(1)K(k)[y(k)CA(1)]}xkkAxkxkxkKalman滤波在自由落体运动目标跟踪问题：functionmainN=1000;%%%%%%%%仿真时间，时间序列总数Q=[0,0;0,0];%%%%过程噪声方差为0，即下落过程忽略空气阻力R=1;%%%%%%%%%%%%观测噪声方差W=sqrt(Q)*randn(2,N);%%既然Q为0，则W=0V=sqrt(R)*randn(1,N);%%%%测量噪声V(k)A=[1,1;0,1];%%%系统矩阵，状态转移矩阵B=[0.5;1];%%%%控制量H=[1,0];%%%%观测矩阵X=zeros(2,N);%%%初始化，物体真实状态X(:,1)=[95;1];%%%初始位移和速度P0=[10,0;0,1];%%%初始误差Z(1)=H*X(:,1);%%%%初始观测值Xkf=zeros(2,N)%%%%kalman估计状态初始化MATLAB仿真程序及结果fork=2:NX(:,k)=A*X(:,k-1)+B*U+W(k);%%%物体下落，受状态方程的驱动Z(k)=H*X(:,k)+V(k);%%%位移传感器对目标进行观测X_pre=A*Xkf(:,k-1)+B*U;%%%状态预测P_pre=A*P0*A‘+Q;%%%协方差预测Kg=P_pre*H‘*inv(H*P_pre*H’+R);%%%计算卡尔曼增益Xkf(:,k)=X_pre+Kg*(Z(k)-H*X_pre);%%%状态更新P0=(I-Kg*H)*P_pre;%%%方差更新err_P(k,1)=P0(1,1);%%%%误差均方值err_P(k,2)=P0(2,2);endmessure_err_x=zeros(1,N);%%%误差计算，位移的侧量误差kalman_err_x=zeros(1,N);%%%估计的位移与真实位移之间的偏差kalman_err_v=zeros(1,N);%%%估计速度与真实速度之间的偏差MATLAB仿真程序及结果本例子中忽略空气噪声等影响，即过程噪声为Q=0。测量噪声：MATLAB仿真程序及结果Kalman滤波算法对噪声的处理：观测位置，滤波位置与真实值之间的偏差速度与真实值之间的偏差MATLAB仿真程序及结果Kalman滤波算法在各个时刻状态的协方差：位移均方差速度均方差MATLAB仿真程序及结果