随机系统的滤波与控制.docx

问题描述：2个传感器在二维平面内跟踪同一个目标，目标做匀速直线运动，运动状态，我们假设目标的初始状态为，用卡尔曼算法对目标轨迹进行估计滤波，假设过程噪声服从。2个传感器的量测误差分别服从和的正态分布，连续测量50个点。并对整个过程进行500次蒙特卡洛仿真。算法本学期我重点学习了卡尔曼滤波算法，卡尔曼滤波的发展过程可以追溯到上世纪40年代的，当时由美国科学家Wiener和前苏联科学家Ｋолмогоров等人提出了最佳线性滤波理论，也即后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。现设线性时变系统的离散状态防城和观测方程为：X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)Y(k) = H(k)·X(k)+N(k)其中X(k)和Y(k)分别是k时刻的状态矢量和观测矢量F(k,k-1)为状态转移矩阵U(k)为k时刻动态噪声T(k,k-1)为系统控制矩阵H(k)为k时刻观测矩阵N(k)为k时刻观测噪声则卡尔曼滤波的算法流程为：预估计X(k)^= F(k,k-1)·X(k-1)?计算预估计协方差矩阵C(k)^=F(k,k-1)×C(k)×F(k,k-1)+T(k,k-1)×Q(k)×T(k,k-1)Q(k) = U(k)×U(k)?计算卡尔曼增益矩阵K(k) = C(k)^×H(k)×[H(k)×C(k)^×H(k)+R(k)]^(-1)R(k) = N(k)×N(k)?更新估计X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]?计算更新后估计协防差矩阵C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]+K(k)×R(k)×K(k)?X(k+1) = X(k)~C(k+1) = C(k)~重复以上步骤仿真图1卡尔曼滤波结果图2 部分放大结果图3 位置误差由图1我们可以看出，该目标的航迹斜率近似等于4，与假设条件目标以速度（1,4）从初始位置（3,0）做匀速直线运动吻合。但是从图2我们可以发现，由于量测过程中存在噪声干扰，因而2个传感器对目标的轨迹预测出现了不同程度的偏差。由之前的问题中我们可以看到，传感器2的误差相比于传感器1的误差要大得多，因而从图3的结果中我们可以明显看到，对两个传感器量测过程做均方跟误差计算，传感器2对位置的预测要更加精确，这是符合实际情况的。总结从上面的仿真我们可以发现，卡尔曼滤波算法只利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值，而无需依赖更早之前的历史数据，因而其更适合用在实时处理问题上，仿真结果表明，卡尔曼滤波算法能对目标轨迹进行较为理想的估计。但是，卡尔曼滤波算法也存在一些缺点，例如当量测数目不断增加时，按滤波方程计算出的估计误差协方差阵趋于零或某一稳定值，但实际的估计误差却越来越大，使滤波器逐渐失去估计作用，这种现象称为滤波器的发散，引起这一问题的原因有：1、描述系统动力学特性的数学模型和噪声的统计模型不准确，不能真实地反映物理过程。这种由于模型过于粗糙或失真引起的发散称为滤波发散。2、卡尔曼滤波是递推过程，随着步数增加，舍入误差不断积累，使增益阵失去合适的加权作用导致发散（计算发散）。现在已经有相应的改进算法来解决这一问题。例如衰减记忆滤波（逐渐减小陈旧量测值的权重，相应地增大新量测值的权重），有限记忆滤波算法（原始 kalman 对量测数据的记忆是无限保存的，有限记忆滤波基于最近一段时间的量测历史），协方差平方根滤波，自适应滤波等，称为次优线性滤波。但是限于时间关系，本文并未对这些方法进行仿真，在未来的工作中，我将对这些方法进行试验研究，比较各种方法的不同。本学期，在蔡老师的引领下，我们得以接触学习各种估计滤波方法，通过一学期的学习，使我们对于控制领域及其研究方法有了更加深入的体会，了解了不同的线性以及非线性滤波技术，这对我们之后的研究生生活有极大的帮助，谢谢老师！Matlab程序clc;clear;close all;T=1;M=500;N=50;F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];H=[1 0 0 0;0 0 1 0];L=[T^2/2 0;T 0;0 T^2/2;0 T];a=[4 0;0 4];b=[8 0;0 8];Q = [2 0;0 2]; X1_esti