状态最优估计Kalman滤波算法.docVIP

状态最优估计的Kalman滤波算法 【摘要】：Kalman滤波器的提出，是为了解决最优控制无法实现全部状态均用于反馈。对于那些无法反馈的状态，则需要根据测量到的信号对其进行估计，再作为反馈。Kalman滤波器的突破点在于采用状态空间，把信号作为白噪声作用的线性输出，并且是将概率论与数理统计的新成果引用与问题的求解。本文中详细论述了Kalman滤波器的推导过程和其计算公式，以及对其过程进行了仿真。状态估计，Kalman滤波器，最优控制 在一定条件下，最优控制保证系统是渐近稳定的，而且具有令人满意的性能。但是它要求全部状态均可用于反馈，这在实际上是难以做到的。实际上常常只能测量系统的一部分状态，而且在测量到的信号中还可能包含有测量噪声。因此需首先根据量测到的信号估计出全部状态，然后按照最优控制规律反馈估计的状态。对于最优估计，Kalman给出了适合计算机计算的状态最优估计递推算法，即Kalman滤波器。针对连续系统和离散系统，Kalman滤波问题又有着连续和离散Kalman滤波器的提法，本文将主要讨论离散模型。 其中x(k)为n维状态向量，u(k)为m维控制向量，y(k)为r维输出向量，v(k)为n维过程干扰向量，w(k)为r维测量噪声向量。假设v(k)和w(k)均为离散的高斯白噪声序列，且有 其中： 同时设V为非负定对称阵，W为正定对称阵，并设v(k)和w(k)不相关。这里我们现将v(k)看作白噪声序列，如果对于实际系统，v(k)不是白噪声，可通过功率谱密度等将其化为标准形式，这个在这里就不再赘述。 在方程(1)中存在随机的干扰v(k)和随机的测量噪声w(k) ，因此系统的状态向量x(k)也为随机向量，其中，y(k)是能够量测的输出量。问题是根据量测量y(k)估计出x(k)。若记x(k)的估计量为，则 为状态估计误差，因而 为状态估计误差的协方差阵，显然P(k)为非负定对称阵。这里估计的准则为根据量测量y(k), y(k-1), …最优地估计出，以使P(k)极小〔P(k)是非负定对称阵，因而可以比较其大小〕。这样的估计称为最小方差估计。根据最优估计理论，最小方差估计为： 即x(k)的最小方差估计等于在给定的直到k时刻的所有量测量y的情况下x(k)的条件期望。为了后面推导的方便，下面引入更一般的记号： 根据j和k的大小关系，估计问题也可分为三类[1]： (a) j k 称为预测（或外推）问题； (b) j = k 称为滤波问题； (c) j k 称为平滑（或内插）问题。 为便于推导，我们引入如下记号： 三、Kalman一步预报估计递推公式 推导Kalman滤波器方程，我们可以先推导一步预报的方程，根据式(1)和(7)得： 根据定义很容易求得一步预报方程： 根据式(1)和(9)，可以得到一步预报误差： 进一步求得一步预报估计误差的协方差阵： 经展开整理可得： 设x(k)的最小方差估计具有如下的形式： 其中K(k)称为状态估计器或Ka1man滤波增益矩阵。该估计器方程具有明显的物理意义，式中第一项是x(k)的一步最优预报估计，它是根据直到k-1时刻的所有量测量的信息而得到x(k)的相关的最优估计。式中第二项是修正项，它根据最新的量测量信息y(k)来对最优预报估计进行修正。在第二项中是关于y(k)的一步预报估计，是关于y(k)的一步预报估计误差，也称新息（Innovation）。所以(13)式可以看作是一步最优预报和新息的加权平均。问题随之变成如何合适地选择K(k)。为获得x(k)的最小状态估计，令状态估计误差的协方差： 最小。现在的问题变为：寻求K(k)使P(k)极小。可以证明，使P(k)极小等价于使如下的标量函数： 极小，其中J表示x(k)的各个分量的方差之和，因而它是标量。下面即按此准则来寻求K(k)。根据式(1)和(13)，可以求得x(k)的状态估计误差为： 进一步求得协方差阵： (17) 可利用摄动法来寻找最优的K(k)，即给K(k)一个增量ΔK(k) ΔP(k)，根据上式忽略高次项，可以求得： 其中： 如果K(k)能够使得式(17)中的P(k)取极小值，那么，对于任意的增量ΔK(k)均应有ΔP(k) = 0。要使该点成立，则必须有 即： 最后，归纳上边(9)(13)(21)(12)(17)即构成了Ka1man滤波递推公式。关键是Kalman增益矩阵的求解，求得增益后再利用式(9)和(13)便可以轻松的到状态的最优估计了。下边重点对增益的计算进行讨论。 四、增益矩阵计算的迭代法 增益矩阵调可以直接根据