卡尔曼滤波推导思路总结

推导思路一：
(1) 混合高斯
一维高斯函数形式：
(1)N(x,μ,σ)=1σ2πe−(x−μ)22σ2\mathcal N(x,\mu,\sigma)=\frac{1}{\sigma\sqrt{2\pi}}e^-\frac{(x-\mu)^2}{2\sigma^2}\tag1 N(x,μ,σ)=σ2π1e−2σ2(x−μ)2(1)

两个高斯函数相乘（未归一化）：
(2)N(x,μ0,σ0)×N(x,μ1,σ1)=?N(x,μ′,σ′)\mathcal N(x,\mu_{0},\sigma_{0})\times\mathcal N(x,\mu_{1},\sigma_{1})=^?\mathcal N(x,\mu{'},\sigma{'})\tag2 N(x,μ0,σ0)×N(x,μ1,σ1)=?N(x,μ′,σ′)(2)

混合后高斯函数其均值和方差求解如下：

(3)μ′=μ0+σ02(μ1−μ0)σ02+σ12σ′2=σ02−σ04σ02+σ12\begin{aligned} \mu{'}&=\mu_{0}+\frac{\sigma_{0}^{2}(\mu_{1}-\mu_{0})}{\sigma_{0}^{2}+\sigma_{1}^{2}}\\ \sigma{'}^2&=\sigma_{0}^2-\frac{\sigma_{0}^4}{\sigma_{0}^{2}+\sigma_{1}^{2}} \end{aligned}\tag3 μ′σ′2=μ0+σ02+σ12σ02(μ1−μ0)=σ02−σ02+σ12σ04(3)

则上式可简化如下：
(4)k=σ02σ02+σ12μ′=μ0+k(μ1−μ0)σ′2=σ02−kσ02\begin{aligned} k&=\frac{\sigma_{0}^2}{\sigma_{0}^{2}+\sigma_{1}^{2}}\\ \mu{'}&=\mu_{0}+k{(\mu_{1}-\mu_{0})}\\ \sigma{'}^2&=\sigma_{0}^2-k\sigma_{0}^2 \end{aligned}\tag4 kμ′σ′2=σ02+σ12σ02=μ0+k(μ1−μ0)=σ02−kσ02(4)

将上式改写为矩阵形式：
(5)K=Σ0(Σ0+Σ1)−1μ⃗′=u0⃗+K(μ1⃗−u0⃗)Σ′=Σ0−KΣ0\begin{aligned} K&=\Sigma_{0}(\Sigma_{0}+\Sigma_1)^{-1}\\ \vec{\mu}{'}&=\vec{u_0}+ K(\vec{\mu_1}-\vec{u_{0}})\\ \Sigma{'}&=\Sigma_{0}-K\Sigma_{0} \end{aligned}\tag{5} Kμ′Σ′=Σ0(Σ0+Σ1)−1=u0+K(μ1−u0)=Σ0−KΣ0(5)

其中Σi\Sigma_{i}Σi表示协方差矩阵，μi⃗\vec{\mu_{i}}μi表示均值。KKK表示卡尔曼增益。

(2)卡尔曼滤波
假定现有两个分布，一个是预测分布和观测分布：
(6)(μ0,Σ0)=(Hkxk^，HkPkHkK)(μ1,Σ1)=(zk⃗,Rk)\begin{aligned} (\mu_0,\Sigma_{0})&=(H_{k}\hat{x_{k}}，H_kP_kH_{k}^{K})\\ (\mu_1,\Sigma_1)&=(\vec{z_{k}},R_{k}) \end{aligned}\tag6 (μ0,Σ0)(μ1,Σ1)=(Hkxk^，HkPkHkK)=(zk,Rk)(6)
结合公式5，可得如下：
(7)Hkxk′⃗=Hkxk⃗+K(zk⃗−Hkxk^)HkPk′HkT=HkPkHkT−KHkPkHkT\begin{aligned} H_k\vec{x_k{'}}&=H_k\vec{x_k}+K(\vec{z_k}-H_k \hat {x_k})\\ H_kP_k{'}H_k^{T}&=H_kP_kH_k^{T}-KH_kP_kH_k^{T} \end{aligned}\tag7 Hkxk′HkPk′HkT=Hkxk+K(zk−Hkxk^)=HkPkHkT−KHkPkHkT(7)

卡尔曼增益如下：
(8)K=HkPkHkT(HkPkHkT+Rk)−1K=H_kP_kH_k^{T}(H_kP_kH_k^{T}+R_k)^{-1}\tag8 K=HkPkHkT(HkPkHkT+Rk)−1(8)

式(7)、式(8)等式左右约去HkH_kHk，注意约去KKK中隐藏的HkH_kHk，可将式(7)、式(8)写为如下：

(9)x⃗k′=xk⃗+K′(zk⃗−Hkxk^)Pk′=Pk−K′HkPkK=PkHkT(HkPkHkT+Rk)−1\begin{aligned} \vec{x}_k{'}&=\vec{x_k}+K{'}(\vec{z_k}-H_k \hat {x_k})\\ P_k{'}&=P_k-K{'}H_kP_k\\ K&=P_kH_k^{T}(H_kP_kH_k^{T}+R_k)^{-1} \end{aligned}\tag9 xk′Pk′K=xk+K′(zk−Hkxk^)=Pk−K′HkPk=PkHkT(HkPkHkT+Rk)−1(9)

式(9)便给了我们完整的更新步骤。

同时，我们附上一下的两个公式：
(10)x^k=Fkx^k−1+Bkμ⃗kPk=FkPk−1FkT+Qk\begin{aligned} \hat x_{k}&=F_k\hat{x}_{k-1}+B_k\vec\mu_k\\ P_k&=F_kP_{k-1}F_k^{T}+ Q_k \end{aligned}\tag{10} x^kPk=Fkx^k−1+Bkμk=FkPk−1FkT+Qk(10)

以上便是经典卡尔曼滤波中相关的5个公式。式(9)和式(10)中部分定义如下：

HkH_kHk为转换矩阵
BkB_kBk为控制矩阵
RkR_kRk为测量误差
QkQ_kQk为系统误差
μ⃗k\vec\mu_kμk为控制向量

流程图如下：

推导思路一细节参见：http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mjx-eqn-update

推导思路二：
由博文可知有预测方程和测量方程如下：
(1){xk=Axk−1+Buk−1+wk−1zk=Hxk+vk\begin{cases} x_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_k=Hx_k+v_k \end{cases}\tag1 {xk=Axk−1+Buk−1+wk−1zk=Hxk+vk(1)

公式1中，AAA表示状态转换矩阵，BBB表示控制矩阵，HHH也表示转换矩阵，xkx_kxk表示预测值，zkz_kzk表示测量值，uku_kuk表示控制向量，wk−1w_{k-1}wk−1表示系统噪声，vkv_kvk表示测量噪声。

针对小车加速运行问题，上式可表示为：

(2){[xtx˙t]=[1Δt01][xt−1x˙t−1]+[Δt22Δt]×αzk=[10][xkx˙k]\begin{cases} \begin{bmatrix} x_t\\ \dot x_t\end{bmatrix}=\begin{bmatrix}1&\Delta t \\0&1\end{bmatrix}\begin{bmatrix} x_{t-1} \\ \dot x_{t-1}\end{bmatrix}+\begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix}\times \alpha\\ z_k=\begin{bmatrix} 1&0\end{bmatrix}\begin{bmatrix} x_k\\ \dot x_k\end{bmatrix} \end{cases}\tag2 ⎩⎪⎪⎨⎪⎪⎧[xtx˙t]=[10Δt1][xt−1x˙t−1]+[2Δt2Δt]×αzk=[10][xkx˙k](2)

公式2中α\alphaα表示加速度。

假定系统噪声wkw_kwk和测量噪声vkv_kvk皆服从于高斯分布，即p(w)∼N(0,Q)p(w)\sim N(0,Q)p(w)∼N(0,Q)和p(v)∼N(0,R)p(v)\sim N(0,R)p(v)∼N(0,R)。以Q=[0000.01]Q=\begin{bmatrix}0&0\\0&0.01\end{bmatrix}Q=[0000.01]为例，QQQ表明系统误差的协方差速度的方差为0.01，位移上标准差为0，速度和方差之间无关联。

现有xk^′\hat{x_k}{'}xk^′为预测值(先验)，x^k\hat{x}_kx^k为估计值，z^k\hat z_kz^k为观测值(后验)。由一般的反馈思想有：

(3)x^k=x^k′+Kk(zk−z^k)=x^k′+Kk(zk−Hx^k′)\begin{aligned} \hat x_k&=\hat x_k{'}+K_k(z_k-\hat z_k)\\&=\hat x_k{'}+K_k(z_k-H\hat x_k{'}) \end{aligned}\tag3 x^k=x^k′+Kk(zk−z^k)=x^k′+Kk(zk−Hx^k′)(3)

其中zkz_kzk为真实值，(zk−Hx^k′)(z_k-H\hat x_k{'})(zk−Hx^k′)为测量值与真实值之间的残差，其中求取KkK_kKk为关键。

假定估计值与真实值之间的协方差为：

(4)Pk=E[ekekT]=E[(xk−x^k)(xk−x^k)T]=[E(SerrSerrT)E(SerrVerrT)E(VerrSerrT)E(VerrVerrT)]\begin{aligned} P_k&=E[e_ke_k^{T}]=E[(x_k-\hat x_k)(x_k-\hat x_k)^T]\\ &=\begin{bmatrix}E(S_{err}S_{err}^T)&E(S_{err}V_{err}^T)\\ E(V_{err}S_{err}^T)&E(V_{err}V_{err}^T)\end{bmatrix} \end{aligned}\tag4 Pk=E[ekekT]=E[(xk−x^k)(xk−x^k)T]=[E(SerrSerrT)E(VerrSerrT)E(SerrVerrT)E(VerrVerrT)](4)

上式中SerrS_{err}Serr表示位移误差，VerrV_{err}Verr表示速度误差。

将式(3)代入式(4)得：
(5)Pk=[(I−KkH)(xk−x^k′)−Kkvk][(I−KkH)(xk−x^k′)−Kkvk]TP_k=[(I-K_kH)(x_k-\hat x_k{'})-K_kv_k][(I-K_kH)(x_k-\hat x_k{'})-K_kv_k]^{T}\tag{5} Pk=[(I−KkH)(xk−x^k′)−Kkvk][(I−KkH)(xk−x^k′)−Kkvk]T(5)

同理得到预测值与真实值之间的协方差。

(6)Pk′=E[ek′ek′T]=E[(xk−x^k′)(xk−x^k′)T]P_k{'}=E[e_k{'}e_k{'}^{T}]=E[(x_k-\hat x_k{'})(x_k-\hat x_k{'})^{T}]\tag6 Pk′=E[ek′ek′T]=E[(xk−x^k′)(xk−x^k′)T](6)

注意系统状态xkx_kxk与测量噪声vkv_kvk之间是相互独立的。

将式(5)展开可得：

(7)Pk=(I−KkH)E[(xk−x^k′)(xk−x^k′)T](I−KkH)T+KkE[vkvkT]KkT=(I−KkH)Pk′(I−KkH)T+KkRKkT=Pk′−KkHPk′−Pk′HTKkT+Kk(HPk′HT+R)KkT\begin{aligned} P_k&=(I-K_kH)E[(x_k-\hat x_k{'})(x_k-\hat x_k{'})^{T}](I-K_kH)^{T}+K_kE[v_kv_k^{T}]K_k^{T}\\ &=(I-K_kH)P_k{'}(I-K_kH)^{T}+K_kRK_k^{T}\\ &=P_k{'}-K_kHP_k{'}-P_k{'}H^{T}K_k^{T}+K_k(HP_k{'}H^T+R)K_k^{T} \end{aligned}\tag7 Pk=(I−KkH)E[(xk−x^k′)(xk−x^k′)T](I−KkH)T+KkE[vkvkT]KkT=(I−KkH)Pk′(I−KkH)T+KkRKkT=Pk′−KkHPk′−Pk′HTKkT+Kk(HPk′HT+R)KkT(7)

结合均方差的意义，利用矩阵的迹对式(7)进行操作得：

(8)T[Pk]=T[Pk′]−2T[KkHPk′]+T[Kk(HPk′HT+R)KkT]\begin{aligned} T[P_k]=T[P_k{'}]-2T[K_kHP_k{'}]+T[K_k(HP_k{'}H^T+R)K_k^{T}] \end{aligned}\tag8 T[Pk]=T[Pk′]−2T[KkHPk′]+T[Kk(HPk′HT+R)KkT](8)

最小均方差，对KkK_kKk进行求导，令导函数为0。则有下式：

(9)dT[Pk]dKk=−2(HPk′)T+2Kk(HPk′HT+R)=0∴Kk=Pk′HT(HPk′HT+R)−1\begin{aligned} \frac{dT[P_k]}{dK_k}&=-2(HP_k{'})^{T}+2K_k(HP_k^{'}H^T+R)=0\\ \therefore K_k&=P_k{'}H^{T}(HP_{k}'H^{T}+R)^{-1} \end{aligned}\tag9 dKkdT[Pk]∴Kk=−2(HPk′)T+2Kk(HPk′HT+R)=0=Pk′HT(HPk′HT+R)−1(9)

其中RRR为测量噪声协方差矩阵。假定上面的所有维度都为1×11\times11×1维，令H=1H=1H=1，且Pk′≠0P_{k}{'}\neq0Pk′̸=0，则公式(9)可以简化如下：
Kk=Pk′Pk′+R=11+RPk′K_k=\frac{P_k{'}}{P_k{'}+R}=\frac{1}{1+\frac{R}{P_k{'}}} Kk=Pk′+RPk′=1+Pk′R1

分析上式可得以下结论：

若KkK_kKk随着Pk′P_k{'}Pk′增大而增大，说明卡尔曼增益越大，越重视反馈。
若Pk′=0P_k{'}=0Pk′=0，说明预测值等于真实值。
若Kk=0K_k=0Kk=0，说明估计值等于预测值。
注意，PkP_kPk为估计值与真实值之间的协方差，Pk′P_k{'}Pk′为预测值与真实值之间的误差。

将计算出的KkK_kKk反代入公式 7中,化简可得：

(10)Pk=Pk′−Pk′HT(HPk′HT+R−1)HPk′=Pk′−KkHPk′=(I−KkH)Pk′\begin{aligned} P_k&=P_k{'}-P_k{'}H^T(HP_k{'}H^{T}+R^{-1})HP_k{'}\\ &=P_k{'}-K_kHP_k{'}\\ &=(I-K_kH)P_k{'} \end{aligned}\tag{10} Pk=Pk′−Pk′HT(HPk′HT+R−1)HPk′=Pk′−KkHPk′=(I−KkH)Pk′(10)

上式中Pk′P_k{'}Pk′的递推计算如下，注意Pk′P_k{'}Pk′为预测值与真实值之间的协方差矩阵。

首先有预测值的递推形式： x^′k+1=Axk^+Buk\hat x{'}_{k+1}=A\hat{x_k}+Bu_kx^′k+1=Axk^+Buk，结合公式(1)可得：

(11)P′k+1=E[e′k+1e′k+1T]=E[(xk+1−x^′k+1)(xk+1−x^′k+1)T]=E[[A(xk−x^k)+wk][A(xk−x^k)+wk]T]\begin{aligned} P{'}_{k+1}&=E[e{'}_{k+1}e{'}_{k+1}^{T}]=E[(x_{k+1}-\hat{x}{'}_{k+1})(x_{k+1}-\hat{x}{'}_{k+1})^{T}]\\ &=E[[A(x_k-\hat x_k)+w_k][A(x_k-\hat x_k)+w_k]^T] \end{aligned}\tag{11} P′k+1=E[e′k+1e′k+1T]=E[(xk+1−x^′k+1)(xk+1−x^′k+1)T]=E[[A(xk−x^k)+wk][A(xk−x^k)+wk]T](11)

注意系统状态xkx_kxk和系统噪声之间相互独立。

所以公式(11)可简化如下：

(12)P′k+1=E[(Aek+1)(Aek+1)T]+E[wkwkT]=APkAT+Q\begin{aligned} P{'}_{k+1}&=E[(Ae_{k+1})(Ae_{k+1})^{T}]+E[w_kw_k^T]\\ &=AP_kA^T+Q \end{aligned}\tag{12} P′k+1=E[(Aek+1)(Aek+1)T]+E[wkwkT]=APkAT+Q(12)

由此，也获得了P′k+1P{'}_{k+1}P′k+1的递推公式。仅需设置最初的PkP_kPk，便能迭代下去。其中QQQ表示系统噪声协方差矩阵。

现将卡尔曼滤波推导思路二总结如下：

(13){x^k−=Ax^k−1+Buk−1Pk−=APk−1AT+Q\begin{cases} \hat x_k^-=A\hat x_{k-1}+Bu_{k-1}\\ P_k^-=AP_{k-1}A^{T}+Q \end{cases}\tag{13} {x^k−=Ax^k−1+Buk−1Pk−=APk−1AT+Q(13)

由公式(13)便可计算卡尔曼增益和估计值如下：

(14){Kk=Pk−HT(HPk−HT+R)−1x^k=x^k−+Kk(zk−Hxk−)\begin{cases} K_k=P_k^{-}H^T(HP_k^{-}H^T+R)^{-1}\\ \hat x_k=\hat x_k^{-}+K_k(z_k-Hx_k^{-}) \end{cases}\tag{14} {Kk=Pk−HT(HPk−HT+R)−1x^k=x^k−+Kk(zk−Hxk−)(14)

最后计算估计值和真实值之间的误差协方差矩阵为下次递推作准备：

Pk=(I−KkH)Pk−P_k=(I-K_kH)P_k^- Pk=(I−KkH)Pk−

推导思路一细节参见：https://blog.csdn.net/heyijia0327/article/details/17487467#commentBox

以上便是对卡尔曼滤波公式推导的两种总结。。。

卡尔曼滤波推导思路总结相关推荐

【核心内容及推导思路】人类记忆系统之谜，也许就是这么回事儿
文章目录 0. 前言 1. 推导思路第1步(猜想的由来及核心内容): 第2步(解剖学上的"疑似证据"): 记忆输入通路示意图记忆检出通路示意图第3步(记忆特征上的" ...
自用的卡尔曼滤波推导
文章目录公式与符号说明几个性质及部分证明高斯白噪声中的直流电平卡尔曼滤波正式推导参考本文从联合正态分布的一些性质出发推导标量和向量形式的卡尔曼滤波,以及重点推导了均值不为0的各个公式.下面 ...
MIT6.824-lab2A-2022篇（万字推导思路及代码构建）
目录前言一.学习背景二.实验引入三.结构体实现 3.1 State的定义 3.2 AppendEntries RPC的定义 3.3 RequestVote RPC的定义四.领导选举 4.1初 ...
MIT6.824-lab2B-2022篇（万字推导思路及代码构建）
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录前言一.整体流程思路二.初始化,发送ticker 2.1.初始化 2.2.发送ticker 三.进行日志增量的RPC 3 ...
lm曲线公式推导_菜鸟理解的IS-LM曲线推导思路，求指正(更新中)
小弟最近备考CFA一级,十二月份考,然后现在看到经济学宏观部分,看到了IS-LM曲线的推导,十分不解,以下是小弟理解的过程,希望大家批评指正,在这里错好过在考试的时候犯错,最后祝大家都顺利通过. 因为 ...
主流卡尔曼滤波推导——KF、EKF、IKF、UKF、ESKF
文章目录一.高斯分布 1.1 高斯概率密度函数 1.2 联合高斯概率密度函数 1.3 高斯随机变量的线性变换二.滤波器基本原理 2.1 贝叶斯滤波三.卡尔曼滤波 3.1 普通卡尔曼滤波器 (KF ...
变分法求解最优控制问题推导思路
目录 1. 最优控制问题 1.1 问题定义 1.2 问题组成及描述 2. 控制问题与变分法 2.1目标函数是泛函 2.2 泛函变分 2.3 泛函极值定理 3. 变分法求解 3.1 广义泛函积分型目标 ...
概率机器人总结——(扩展)卡尔曼滤波先实践再推导
概率机器人总结--卡尔曼滤波先实践再推导概率机器人总结--(扩展)卡尔曼滤波先实践再推导 (1)卡尔曼.扩展卡尔曼.粒子滤波到底什么关系? (2)实践--扩展卡尔曼滤波 (3)推导--卡尔曼滤波 ( ...
卡尔曼滤波的推导过程详解
在学习卡尔曼滤波的时候看了很多博客讲这方面的知识,感觉都讲得表面的东西,无法了解它五个公式真正代表的过程,这篇博客我想以我的理解讲讲卡尔曼滤波. 首先我先写出卡尔曼滤波的具体过程,首先针对如下状态空间 ...

卡尔曼滤波推导思路总结

卡尔曼滤波推导思路总结相关推荐

最新文章

热门文章