无人机常用算法——卡尔曼滤波器（四）

sigma

无人机常用算法——卡尔曼滤波器（四） [复制链接]

2.2 卡尔曼滤波基本公式

设系统的状态方程为：

X (k) = AX (k −1) + BU (k −1) +W (k −1) （1）

系统的测量方程：

Z (k) = CX (k) + V (k) （2）

其中：A(k-1)为为状态转移矩阵；X(k-1)为状态向量；B(k-1)为输入控制项矩阵；u(k-1)为输入或者控制信号；W(k-1)为零均值、白色高斯过程噪声序列，其协方差为 Q(k-1)；C(k)为测量矩阵；V(k)为协方差为 R(k)的零均值、白色高斯测量噪声。系统的测量方程的输出项 Z(k)是可以实际测量的量。用

表示 k 时刻对随机信号 X(k)的最优线性滤波估计值，用

表示在 k时刻对 k＋1时刻的信号X（k＋1）的最优线性预测估计。

卡尔曼滤波器的递推算法如下：

协方差的更新方程还有下面几种不同的形式：

P(k | k) = P(k | k −1) − K (k)C(k)P(k | k −1) (e1)

式（e）和（e1）相同，适用于增益为最优卡尔曼增益的时候。使用其它增益的话要用公式（e3）。

(e2)

其中，测量的预测协方差（或新息协方差）

用来衡量新息（测量值减去测量估计值）的不确定性，新息的协方差越小，说明测量值越精确。不知道公式（e2）有什么应用条件，但是应该比公式（e1）适应性好，该书中就使用该式进行卡尔曼滤波迭代。

(e3)

其中，I 为与协方差阵同维的单位阵。式（e3）可保证协方差阵 P 的对称性和正定性。这个公式对于任何卡尔曼增益 K 都成立。

式（e）或（e1）计算比较简单，所以实际中总是使用这个公式，但是需要注意这个公式仅在使用最优卡尔曼增益时它才成立。如果算术精度总是很低而导致 numerical stability 出现问题，或者特意使用了非最优卡尔曼增益，就必须使用式（e3）。但是什么是最优卡尔曼增益还不大清楚，可以先试用简单的形式。

上面(a)~(e)的 5 个式子就是卡尔曼滤波的迭代公式。如果系统是非时变的，则系数矩阵 A、B、C 都是常数矩阵。根据上面的 5 个公式就可以完成卡尔曼滤波的全过程并继续下去。因为把根据状态方程计算的预测值

和 k 时刻的实际测量值 Z(k)相结合来估计 k 时刻的状态真值 X(k|k)，所以起到了削减误差的最佳估计效果，实现了测量值的修正。

过程（状态）协方差矩阵 P

两个标量x1，x2标准的协方差公式为cov(x1 , x2 ) = E{[x1 − E(x1 )][x2− E(x2 )]} 。

设状态向量 X 有 q 个分量，一步滤波后各个状态分量的误差:

该误差的均方误差 P(k)就变成一个误差的协方差矩阵