卡尔曼滤波


声明:本笔记仅仅为本人学习卡尔曼滤波时所总结,仅供交流学习,侵删。 \bf{声明:本笔记仅仅为本人学习卡尔曼滤波时所总结,仅供交流学习,侵删。} 声明:本笔记仅仅为本人学习卡尔曼滤波时所总结,仅供交流学习,侵删。

1. 定义输入

输入:一个符合高斯分布的已知状态量 x ⃗ k − 1 \vec{x}_{k-1} x k1,且该高斯分布的协方差矩阵为 P k − 1 \bf P_{k-1} Pk1

2. 输入状态量到中间预测状态量

接下来,我们需要根据已知状态量来预测下一个步长的状态 x ⃗ k \vec{x}_{k} x k,我们需要状态转移矩阵 F k \bf F_k Fk和控制矩阵 B k \bf B_k Bk来预测我们的下一个状态 x ⃗ k \vec{x}_{k} x k,并且需要规定我们的控制向量 u ⃗ k \vec{u}_k u k,相关的表达式如下:
x ⃗ k = F k x ⃗ k − 1 + B k u ⃗ k (1) \vec{x}_k=\bf{F_k}\vec{x}_{k-1}+\bf{B_k}\vec{u}_k\tag{1} x k=Fkx k1+Bku k(1)
因为我们对每个点都进行了状态转移的预测,因此协方差矩阵也需要跟着转换,结果为:
P k = F k P k − 1 F k T (2) \bf{P_k}=F_kP_{k-1}F_k^T\tag{2} Pk=FkPk1FkT(2)
证明如下:已知协方差矩阵 Var [ x ] = Cov [ x , x ] = Σ \textit{Var}[\bf{x}]=\textit{Cov}[\bf{x,x}]=\Sigma Var[x]=Cov[x,x]=Σ,那么我们可以得到:
一个已知前提: E [ F x ] = F E [ x ] E\left[Fx\right]=FE[x] E[Fx]=FE[x],证明参考协方差矩阵的简单介绍,因此:
Var [ F x ] = Cov [ F x , F x ] = E { ( F x − E [ F x ] ) ( F x − E [ F x ] ) T } = E { ( F x − F E [ x ] ) ( F x − F E [ x ] ) T } = E { F ( x − E [ x ] ) ( x − E [ x ] ) T F T } = F E { ( x − E [ x ] ) ( x − E [ x ] ) T } F T = F Σ F T (3) \begin{aligned} \textit{Var}[\bf{Fx}]&=\textit{Cov}[\textit{F}x,\textit{F}x]\\ &=\textit{E}\{(Fx-E[Fx])(Fx-E[Fx])^T\}\\ &=\textit{E}\{(Fx-FE[x])(Fx-FE[x])^T\}\\ &=\textit{E}\{F(x-E[x])(x-E[x])^TF^T\}\\ &=F\textit{E}\{(x-E[x])(x-E[x])^T\}F^T\\ &=F\Sigma F^T \end{aligned}\tag{3} Var[Fx]=Cov[Fx,Fx]=E{(FxE[Fx])(FxE[Fx])T}=E{(FxFE[x])(FxFE[x])T}=E{F(xE[x])(xE[x])TFT}=FE{(xE[x])(xE[x])T}FT=FΣFT(3)

3. 考虑外部动力和噪声

x ⃗ k − 1 \vec{x}_{k-1} x k1中的每个点移动到一个符合协方差的 Q k \textit{Q}_k Qk高斯分布里(也就是蓝色的高斯分布移动到紫色高斯分布的位置),或者说,把这些不确定的外部因素描述为协方差为的高斯噪声 Q k \textit{Q}_k Qk,具体根据预测的传感器噪声来定(比如imu或者轮速计):

Q k \textit{Q}_k Qk 简单叠加,可以拿到扩展的协方差,这样就得到了预测步骤的完整表达式:
x ⃗ k = F k x ⃗ k − 1 + B k u ⃗ k P k = F k P k − 1 F k T + Q k (4) \begin{aligned} &\vec{x}_k=\bf{F_k}\vec{x}_{k-1}+\bf{B_k}\vec{u}_k\\ &\bf{P_k}=F_kP_{k-1}F_k^T+\textit{Q}_k \end{aligned}\tag{4} x k=Fkx k1+Bku kPk=FkPk1FkT+Qk(4)

4. 高维高斯分布叠加

上述表达就是关于数学模型的预测方法,但是完成这些是不够的,因为我们知道预测得到的是一个高斯分布,且仅仅只是经过我们一次预测得到的结果,如果我们将更多已知的信息加入到数学模型中,那么我们将得到更加准确的或者说更加符合实际的结果,因此我们结合许多应用卡尔曼滤波的项目中具有大量传感器获取的测量值的情况,并且这些测量值因为有着精度和环境噪声等的影响,我们将它们也当做是符合高斯分布的数学模型,而两个高斯分布我们可以通俗地理解成以下图形:

为了得到更加准确的结果,我们认为两个高斯分布重叠的部分比它们原来两个中的任何一个高斯分布都更加符合实际(最优卡尔曼增益相关证明参考卡尔曼滤波(Kalman Filter)原理与公式推导),因此如何融合两个高斯分布呢?

我们从简单的一维高斯分布来看:
N ( x , μ 0 , σ 0 ) ∗ N ( x , μ 1 , σ 1 ) = 1 σ 0 2 π e − ( x − μ 0 ) 2 2 σ 0 2 ⋅ 1 σ 1 2 π e − ( x − μ 1 ) 2 2 σ 1 2 = 1 2 π 1 2 π σ 0 σ 1 e − ( σ 0 2 + σ 1 2 ) x 2 − 2 ( μ 0 σ 1 2 + μ 1 σ 0 2 ) x + σ 1 2 μ 0 2 + σ 0 2 μ 1 2 2 ( σ 0 σ 1 ) 2 = 1 2 π 1 2 π σ 0 σ 1 e − ( x − μ 0 σ 1 2 + μ 1 σ 0 2 σ 0 2 + σ 1 2 ) 2 2 ( σ 0 σ 1 σ 0 2 + σ 1 2 ) 2 +   σ 1 2 μ 0 2 + σ 0 2 μ 1 2 σ 0 2 + σ 1 2 − ( μ 0 σ 1 2 + μ 1 σ 0 2 σ 0 2 + σ 1 2 ) 2 2 ( σ 0 σ 1 σ 0 2 + σ 1 2 ) 2 ⏟ 常数 λ = ( μ 0 − μ 1 ) 2 2 ( σ 0 2 + σ 1 2 ) = C   1 2 π σ 0 σ 1 σ 0 2 + σ 1 2 e − ( x − μ 0 σ 1 2 + μ 1 σ 0 2 σ 0 2 + σ 1 2 ) 2 2 ( σ 0 σ 1 σ 0 2 + σ 1 2 ) 2 其中, C 代表常数项,从而有 μ ′ = μ 0 σ 1 2 + μ 1 σ 0 2 σ 0 2 + σ 1 2 = μ 0 + ( μ 1 − μ 0 ) σ 0 2 σ 0 2 + σ 1 2 σ ′ 2 = σ 0 2 σ 1 2 σ 0 2 + σ 1 2 = σ 0 2 − σ 0 2 σ 0 2 σ 0 2 + σ 1 2 (5) \begin{aligned} &N(x,\mu_0,\sigma_0)*N(x,\mu_1,\sigma_1)\\ &=\frac{1}{\sigma_0 \sqrt{2\pi}}e^{-\frac{(x-\mu_0)^2}{2\sigma_0^2}}\cdot\frac{1}{\sigma_1 \sqrt{2\pi}}e^{-\frac{(x-\mu_1)^2}{2\sigma_1^2}}\\ &=\frac{1}{\sqrt{2\pi}}\frac{1}{\sqrt{2\pi}\sigma_0\sigma_1}e^{-\frac{(\sigma_0^2+\sigma_1^2)x^2-2(\mu_0\sigma_1^2+\mu_1\sigma_0^2)x+\sigma_1^2\mu_0^2+\sigma_0^2\mu_1^2}{2(\sigma_0\sigma_1)^2}}\\ &=\frac{1}{\sqrt{2\pi}}\frac{1}{\sqrt{2\pi}\sigma_0\sigma_1}e^{-\frac{\left(x-\frac{\mu_0\sigma_1^2+\mu_1\sigma_0^2}{\sigma_0^2+\sigma_1^2}\right)^2}{2\left(\frac{\sigma_0\sigma_1}{\sqrt{\sigma_0^2+\sigma_1^2}}\right)^2}+\,\underbrace{\frac{\frac{\sigma_1^2\mu_0^2+\sigma_0^2\mu_1^2}{\sigma_0^2+\sigma_1^2}-\left(\frac{\mu_0\sigma_1^2+\mu_1\sigma_0^2}{\sigma_0^2+\sigma_1^2}\right)^2}{2\left(\frac{\sigma_0\sigma_1}{\sqrt{\sigma_0^2+\sigma_1^2}}\right)^2}}_{常数\lambda=\frac{(\mu_0-\mu_1)^2}{2(\sigma_0^2+\sigma_1^2)}}}\\ &=C\,\frac{1}{\sqrt{2\pi}\frac{\sigma_0\sigma_1}{\sqrt{\sigma_0^2+\sigma_1^2}}}e^{-\frac{\left(x-\frac{\mu_0\sigma_1^2+\mu_1\sigma_0^2}{\sigma_0^2+\sigma_1^2}\right)^2}{2\left(\frac{\sigma_0\sigma_1}{\sqrt{\sigma_0^2+\sigma_1^2}}\right)^2}}\\ &其中,C代表常数项,从而有\\ &{\mu}'=\frac{\mu_0\sigma_1^2+\mu_1\sigma_0^2}{\sigma_0^2+\sigma_1^2}=\mu_0+(\mu_1-\mu_0)\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2}\\ &{\sigma}'^2=\frac{\sigma_0^2\sigma_1^2}{\sigma_0^2+\sigma_1^2}=\sigma_0^2-\sigma_0^2\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2}\\ \end{aligned}\tag{5} N(x,μ0,σ0)N(x,μ1,σ1)=σ02π 1e2σ02(xμ0)2σ12π 1e2σ12(xμ1)2=2π 12π σ0σ11e2(σ0σ1)2(σ02+σ12)x22(μ0σ12+μ1σ02)x+σ12μ02+σ02μ12=2π 12π σ0σ11e2(σ02+σ12 σ0σ1)2(xσ02+σ12μ0σ12+μ1σ02)2+常数λ=2(σ02+σ12)(μ0μ1)2 2(σ02+σ12 σ0σ1)2σ02+σ12σ12μ02+σ02μ12(σ02+σ12μ0σ12+μ1σ02)2=C2π σ02+σ12 σ0σ11e2(σ02+σ12 σ0σ1)2(xσ02+σ12μ0σ12+μ1σ02)2其中,C代表常数项,从而有μ=σ02+σ12μ0σ12+μ1σ02=μ0+(μ1μ0)σ02+σ12σ02σ′2=σ02+σ12σ02σ12=σ02σ02σ02+σ12σ02(5)
k = σ 0 2 σ 0 2 + σ 1 2 k=\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2} k=σ02+σ12σ02,则:
μ ′ = μ 0 + k ( μ 1 − μ 0 ) σ ′ 2 = σ 0 2 − k σ 0 2   (6) \begin{aligned} &{\mu}'=\mu_0+k(\mu_1-\mu_0)\\ &{\sigma}'^2=\sigma_0^2-k\sigma_0^2\ \end{aligned}\tag{6} μ=μ0+k(μ1μ0)σ′2=σ02kσ02 (6)
如果是一个多维矩阵呢?我们将(6)表示为矩阵形式。 Σ \Sigma Σ表示协方差矩阵, μ ⃗ \vec{\mu} μ 表示平均向量:
K = Σ 0 ( Σ 0 + Σ 1 ) − 1 (7) K=\Sigma_0\left(\Sigma_0+\Sigma_1\right)^{-1}\tag{7} K=Σ0(Σ0+Σ1)1(7)

μ ⃗ ′ = μ ⃗ 0 + K ( μ ⃗ 1 − μ ⃗ 0 ) Σ ′ = Σ 0 − K Σ 0 (8) \begin{aligned} &\vec{\mu}'=\vec{\mu}_0+K\left(\vec{\mu}_1-\vec{\mu}_0\right)\\ &\Sigma'=\Sigma_0-K\Sigma_0 \end{aligned}\tag{8} μ =μ 0+K(μ 1μ 0)Σ=Σ0KΣ0(8)

5. 从状态域转换为观测域

有了以上的基础,我们还需要将预测值转化为与测量值同维度的形式,即预测值在测量维度上的表达可以通过相信变换矩阵 H k H_k Hk相当于状态域通过 H k H_k Hk转换到观测域

μ ⃗ m e a s u r e d = H k ∗ x ⃗ k \vec{\mu}_{measured}=H_k*\vec{x}_k μ measured=Hkx k

Σ m e a s u r e d = H k P k H k T \Sigma_{measured}=H_k\textbf{P}_kH_k^T Σmeasured=HkPkHkT

进而,我们知道:

观测域上的预测值: ( μ 0 , Σ 0 ) = ( H k x ⃗ k , H k P k H k T ) \left(\mu_0,\Sigma_0\right)=\left(H_k\vec{x}_k,H_k\text{P}_kH_k^T\right) (μ0,Σ0)=(Hkx k,HkPkHkT)

实际的测量值: ( μ 1 , Σ 1 ) = ( z k ′ , R k ) \left(\mu_1,\Sigma_1\right)=\left({z}'_k,R_k\right) (μ1,Σ1)=(zk,Rk)

6. 生成新的状态作为下次迭代的输入

将上述两个多维高斯分布代入(8)中,得到它们的重叠区域:
H k x ⃗ k ′ = H k + K ( z ⃗ k − H k x k ⃗ ) H k P k ′ H k T = H k P k H k T − K H k P k H k T (9) H_k\vec{x}'_k=H_k+K\left(\vec{z}_k-H_k\vec{x_k}\right)\\ H_k\textbf{P}'_kH_k^T=H_k\textbf{P}_kH_k^T-KH_k\textbf{P}_kH_k^T\tag{9} Hkx k=Hk+K(z kHkxk )HkPkHkT=HkPkHkTKHkPkHkT(9)
从(7)中我们可以得到卡尔曼增益为:
K = H k P k H k T ( H k P k H k T + R k ) − 1 (10) K=H_k\textbf{P}_kH_k^T\left(H_k\textbf{P}_kH_k^T+R_k\right)^{-1}\tag{10} K=HkPkHkT(HkPkHkT+Rk)1(10)
但是上述式子(9)左侧表示的并不是我们需要的状态估计值 x ⃗ k ′ \vec{x}'_k x k,因此在(9)的第一个式子左右两边各左乘一个 H k − 1 H_k^{-1} Hk1,第二个式子左右两边先左乘一个 H k − 1 H_k^{-1} Hk1然后右乘一个 H k T − 1 {H_k^T}^{-1} HkT1(可调换顺序),这样我们将会得到如下结果:
x ⃗ k ′ = x ⃗ k + K ′ ( z ⃗ k − H k x ⃗ k ) P k ′ = P k − K ′ H k P k = ( E − K ′ H k ) P k (11) \vec{x}'_k=\vec{x}_k+K'\left(\vec{z}_k-H_k\vec{x}_k\right)\\ \textbf{P}'_k=\textbf{P}_k-K'H_k\textbf{P}_k=\left(E-K'H_k\right)\textbf{P}_k\tag{11} x k=x k+K(z kHkx k)Pk=PkKHkPk=(EKHk)Pk(11)

K ′ = P k H k T ( H k P k H k T + R k ) − 1 (12) K'=\textbf{P}_kH_k^T\left(H_k\textbf{P}_kH_k^T+R_k\right)^{-1}\tag{12} K=PkHkT(HkPkHkT+Rk)1(12)

7. 整体流程

所有输入量、中间变量、输出量的关系以及输出量的迭代在下图中被表达出来:

一个简化的流程被表达为:

参考资料:

1.卡尔曼滤波更加详细资料

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值