卡尔曼滤波的状态方程和观测方程如式(1)-(2)。
(1)
式中, 是 时刻状态向量; 是观测向量; 为状态转移矩阵; 为观测设计矩阵; 和 分别称为状态误差和观测误差。
设相邻时刻 、 和 的动态噪声 、 和 互相独立,动态噪声 和观测噪声 互不相关。但 时刻观测噪声 与相邻时刻的观测噪声 相关,不相邻时刻的观测噪声 和 互不相关,即 、 和 的协方差阵(式(3))为:
(3)
设 为高斯白噪声序列, 为有色噪声序列。观测 次可得 的预估值,第 次滤波的预估值为 ,其协方差为 。由于 为有色噪声序列,使得 与 相关,其协方差(式(4))为:
(4)
(5)
(6)
(7)
相邻历元观测噪声相关情况下,第 历元函数模型(式(8))和随机模型(式(9))分别为:
(8)
(9)
相邻历元观测噪声相关情况下第 历元状态估值(式(10))为:
(10)
(11)
(12)
(13)
如果观测向量存在粗差,其抗差卡尔曼滤波迭代公式为式(14)。
(14)
(15)
其中, 按式(16)计算:
(16)
由于有色噪声的观测值相关,则可采用双因子等价协方差矩阵。等价协方差阵 可构造为式(17)。
(17)
式中, 称为抗差因子,可按式(18)计算。
(18)
式中, 和 为抗差阈值,一般分别取为1.5-2.0和3.0-8.5; 为标准化残差。
设系统 时刻的观测误差 与相邻时刻 和 的观测误差 和 互相独立,动态噪声 和观测噪声 互不相关。但 时刻动态误差 与相邻时刻 和 的动态误差 、 相关,不相邻时刻的动态误差 、 互不相关,即 、 和 的协方差阵(式(19))为:
(19)
卡尔曼滤波先需计算状态预报值 (式(20))及其协方差矩阵((式21)):
(20)
(21)
式中, 是由于 造成的 与 的协方差 。由于
(22)
(23)
将式(23)代入式(21)计算 ,并用 、 计算 和 。从而导出有色噪声作用下,卡尔曼滤波的递推公式为(式(24)-(25)):
(24)
(25)
式中, 是动态系统根据状态方程进行的一步预估值; 是新观测值对一步预估值 的修正值, 称为增益矩阵,其计算式为式(26):
(26)
如果状态向量存在粗差,其抗差卡尔曼滤波迭代公式为式(27):
(27)
(28)
按式(21)得
(29)
(30)
式中, 、 按迭代计算。
(3)相邻历元状态误差与观测误差均相关的抗差卡尔曼滤波上述2种误差均为有色误差的抗差卡尔曼滤波时,只要把前2部分结合起来,则可导出相应的迭代公式,迭代公式不再赘述。
等价协方差阵 可构造为式(31):
(31)