卡尔曼滤波器笔记

• 卡尔曼滤波器笔记
  • 1.基本动态系统模型
  • 2.卡尔曼滤波流程
    • 1.1推导最优卡尔曼增益
  • 3.扩展卡尔曼滤波

1.基本动态系统模型

状态转移 xk=Fkxk−1+Bkuk+wk x k = F k x k − 1 + B k u k + w k

测量模型 zk=Hkxk+vk z k = H k x k + v k

x x 表示状态向量， u u 表示控制向量， z z 表示观测向量， w w 表示过程噪声， v v 表示观测噪声。 F F 表示状态转移模型， B B 表示控制-输入模型， H H 表示观测模型。下标 k k 及k−1k−1表示当前时刻以及上一时刻。

2.卡尔曼滤波流程

预测阶段

预测（先验）状态估计 x̂ k|k−1=Fkx̂ k−1|k−1+Bkuk x ^ k | k − 1 = F k x ^ k − 1 | k − 1 + B k u k

预测（先验）估计协方差 Pk|k−1=FkPk−1|k−1FTk+Qk P k | k − 1 = F k P k − 1 | k − 1 F k T + Q k

更新阶段

计算测量余量 ỹ k=zk−Hkx̂ k|k−1 y ~ k = z k − H k x ^ k | k − 1

计算测量余量协方差 Sk=HkPk|k−1HT+Rk S k = H k P k | k − 1 H T + R k

计算最优卡尔曼增益 Kk=Pk|k−1HTkS−1k K k = P k | k − 1 H k T S k − 1

更新状态估计 x̂ k|k=x̂ k|k−1+Kkỹ k x ^ k | k = x ^ k | k − 1 + K k y ~ k

更新估计协方差 Pk|k=(I−KkHk)Pk|k−1 P k | k = ( I − K k H k ) P k | k − 1

下标 k|k k | k 以及 k−1|k−1 k − 1 | k − 1 表示当前估计以及上一时刻估计。下标 k|k−1 k | k − 1 表示当前时刻的预测估计，是一种中间向量或模型。

卡尔曼滤波流程如下图所示。

1.1推导最优卡尔曼增益

卡尔曼滤波器的最优化目的在于最小化估计状态变量 x̂  x ^ 相对于真实状态变量 x x 的均方误差 E[|xk−x̂ k|k|] E [ | x k − x ^ k | k | ] ，等价于最小化估计协方差 Pk|k P k | k 的迹。其公式推导如下：

Pk|k=cov(xk−x̂ k|k)Pk|k=cov(xk−(x̂ k|k−1+Kk(Hkxk+vk−Hkx̂ k|k−1)))Pk|k=(I−KkHk)Pk|k−1(I−KkHk)T+KkRkKTk P k | k = c o v ( x k − x ^ k | k ) P k | k = c o v ( x k − ( x ^ k | k − 1 + K k ( H k x k + v k − H k x ^ k | k − 1 ) ) ) P k | k = ( I − K k H k ) P k | k − 1 ( I − K k H k ) T + K k R k K k T

dtr(Pk|k)dKk=−2(HkPk|k−1)T+2KkSk=0Kk=Pk|k−1HTkS−1k d t r ( P k | k ) d K k = − 2 ( H k P k | k − 1 ) T + 2 K k S k = 0 K k = P k | k − 1 H k T S k − 1

3.扩展卡尔曼滤波

基本卡尔曼滤波受限于线性模型假设。对于非线性模型，利用一阶麦克劳林将其展开近似等效，展开公式中的常数部分对协方差的计算没有贡献，因此可利用Jacobians矩阵代替非线性模型计算协方差以及最优卡尔曼增益。