卡尔曼滤波器笔记
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 及表示当前时刻以及上一时刻。
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矩阵代替非线性模型计算协方差以及最优卡尔曼增益。
947
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
