本文从概率论中的贝叶斯准则和全概率公式出发，详细推导了卡尔曼滤波器的数学模型，包括先验和后验概率的计算，以及卡尔曼增益的推导。
前言：这可能是网上最数学化的推导了，相较于其他的推导方法在数学上更加完备，这意味着对于其他的非线性滤波器如EKF或者信息滤波器IF很多其他地方都可以采用同样的方法进行推导，一通百通。本文的卡尔曼滤波器的推导从最基础的概率论中的贝叶斯准则和全概率公式推起，一步一步的完成卡尔曼滤波器，适合对卡尔曼有一定概念上的理解希望深入学习滤波器方法的同学。
卡尔曼滤波算法
μˉt=Atμt−1+Btut
\bar{\mu}_t = A_t \mu_{t-1} + B_t u_t
μˉt=Atμt−1+Btut
Σˉt=AtΣt−1AtT+Rt
\bar{\Sigma}_t = A_t \Sigma_{t-1} A_t^T + R_t
Σˉt=AtΣt−1AtT+Rt
Kt=ΣˉtCtT(CtΣˉtCtT+Qt)−1
K_t = \bar{\Sigma}_t C_t^T(C_t \bar{\Sigma}_t C_t^T + Q_t)^{-1}
Kt=ΣˉtCtT(CtΣˉtCtT+Qt)−1
μt=μˉt+Kt(zt−Ctμˉt)
\mu_t = \bar{\mu}_t + K_t (z_t-C_t \bar{\mu}_t)
μt=μˉt+Kt(zt−Ctμˉt)
Σt=(I−KtCt)Σˉt
\Sigma_t = (I-K_t C_t)\bar{\Sigma}_t
Σt=(I−KtCt)Σˉt
推导过程：
先验条件
贝叶斯准则：
p(x∣y,z)=p(y∣x,z)p(x∣z)p(y∣z)
p(x|y,z)=\frac{p(y|x,z)p(x|z)}{p(y|z)}
p(x∣y,z)=p(y∣z)p(y∣x,z)p(x∣z)
全概率公式：
p(x)=∫p(x∣y)p(y)dy
p(x) = \int p(x|y)p(y) dy
p(x)=∫p(x∣y)p(y)dy
马尔可夫性：认为xt−1x_{t-1}xt−1是zt−1z_{t-1}zt−1、ut−1u_{t-1}ut−1的最优估计
贝叶斯滤波器推导
p(xt∣z1:t,u1:t)=p(xt∣zt,z1:t−1,u1:t)(套用贝叶斯准则)=p(zt∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)p(zt∣z1:t−1,u1:t)(分母与xt无关)=ηp(zt∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)(马尔可夫性)=ηp(zt∣xt)p(xt∣z1:t−1,u1:t)(区分先验后验)
p(x_t|z_{1:t},u_{1:t})=p(x_t|z_t,z_{1:t-1},u_{1:t}) (套用贝叶斯准则) \\
=\frac{p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})}{p(z_t|z_{1:t-1},u_{1:t})}(分母与x_t无关)\\
=\eta p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t}) (马尔可夫性)\\
=\eta p(z_t|x_t)p(x_t|z_{1:t-1},u_{1:t}) (区分先验后验) \\
p(xt∣z1:t,u1:t)=p(xt∣zt,z1:t−1,u1:t)(套用贝叶斯准则)=p(zt∣z1:t−1,u1:t)p(zt∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)(分母与xt无关)=ηp(zt∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)(马尔可夫性)=ηp(zt∣xt)p(xt∣z1:t−1,u1:t)(区分先验后验)
对于先验分布p(xt∣z1:t−1,u1:t)p(x_t|z_{1:t-1},u_{1:t})p(xt∣z1:t−1,u1:t)带入全概率公式
bel‾(xt)=p(xt∣z1:t−1,u1:t)=∫p(xt∣xt−1,z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1
\overline{bel}(x_t) = p(x_t|z_{1:t-1},u_{1:t}) \\
= \int p(x_t|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1},u_{1:t})d x_{t-1}
bel(xt)=p(xt∣z1:t−1,u1:t)=∫p(xt∣xt−1,z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1
最终得到先验和后验公式
先验：
bel‾(xt)=∫p(xt∣xt−1,z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1
\overline{bel}(x_t)
= \int p(x_t|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1},u_{1:t})d x_{t-1}
bel(xt)=∫p(xt∣xt−1,z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1
后验：
bel(xt)=ηp(zt∣xt)bel‾(xt)
bel(x_t)=\eta p(z_t|x_t)\overline{bel}(x_t)
bel(xt)=ηp(zt∣xt)bel(xt)
卡尔曼滤波器推导
根据贝叶斯滤波器带入线性的模型，推导得到卡尔曼滤波器。
模型
预测方程：
xt=Atxt−1+Btut+ϵt
x_t=A_t x_{t-1} + B_t u_t + \epsilon_t
xt=Atxt−1+Btut+ϵt
xt=(x1,tx2,t⋮xn,t)ut=(u1,tu2,t⋮um,t)
x_{t}=\left(\begin{array}{c}
x_{1, t} \\
x_{2, t} \\
\vdots \\
x_{n, t}
\end{array}\right)
\boldsymbol{u}_{t}=\left(\begin{array}{c}
u_{1, t} \\
u_{2, t} \\
\vdots \\
u_{m, t}
\end{array}\right)
xt=⎝⎜⎜⎜⎛x1,tx2,t⋮xn,t⎠⎟⎟⎟⎞ut=⎝⎜⎜⎜⎛u1,tu2,t⋮um,t⎠⎟⎟⎟⎞
观测方程：
zt=Ctxt+δt
z_{t}=C_{t} x_{t}+\delta_{t}
zt=Ctxt+δt
预测
根据贝叶斯滤波器的先验公式
bel‾(xt)=∫p(xt∣xt−1,ut)⏟∼N(xt;Atxt−1+Btut,Rt)bel(xt−1)⏟∼N(xt−1;μt−1,Σt−1)dxt−1\overline{\mathrm{bel}}\left(x_{t}\right)=\int \underbrace{p\left(x_{t} | x_{t-1}, u_{t}\right)}_{\sim N\left(x_{t} ; A_{t} x_{t-1}+B_{t} u_{t}, R_{t}\right)} \underbrace{\mathrm{bel}\left(x_{t-1}\right)}_{\sim N\left(x_{t-1} ; \mu_{t-1}, \Sigma_{t-1}\right)} \mathrm{d} x_{t-1}bel(xt)=∫∼N(xt;Atxt−1+Btut,Rt)p(xt∣xt−1,ut)∼N(xt−1;μt−1,Σt−1)bel(xt−1)dxt−1
其中，t-1时刻bel(xt−1)bel(x_{t-1})bel(xt−1)的概率分布为高斯分布，均值为μt−1\mu_{t-1}μt−1，方差为Σt−1\Sigma_{t-1}Σt−1。
bel‾(xt)=η∫exp{−12(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)}exp{−12(xt−1−μt−1)TΣt−1−1(xt−1−μt−1)}
\overline{bel}(x_t) = \eta \int exp \left \{ -\frac{1}{2} (x_t-A_t x_{t-1}-B_t u_t)^TR_{t}^{-1}(x_t-A_t x_{t-1}-B_t u_t)\right\}exp\left \{ -\frac{1}{2}(x_{t-1}-\mu_{t-1})^T \Sigma_{t-1}^{-1}(x_{t-1}-\mu_{t-1})\right\}
bel(xt)=η∫exp{−21(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)}exp{−21(xt−1−μt−1)TΣt−1−1(xt−1−μt−1)}
仅考虑指数部分，记为bel‾(xt)=η∫exp{−Lt}dxt−1\overline{bel}(x_t)=\eta \int exp\{-L_t \}dx_{t-1}bel(xt)=η∫exp{−Lt}dxt−1,其中
Lt=12(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)+12(xt−1−μt−1)TΣt−1−1(xt−1−μt−1)
L_t = \frac{1}{2} (x_t-A_t x_{t-1}-B_t u_t)^TR_{t}^{-1}(x_t-A_t x_{t-1}-B_t u_t) + \frac{1}{2}(x_{t-1}-\mu_{t-1})^T \Sigma_{t-1}^{-1}(x_{t-1}-\mu_{t-1})
Lt=21(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)+21(xt−1−μt−1)TΣt−1−1(xt−1−μt−1)
对于xt−1x_{t-1}xt−1的积分的计算则考虑到概率分布在整个空间中积分为常数，由于LtL_tLt是关于$x_{t-1}的二次型，因此关于其的分布为高斯分布。高斯分布的计算只需要求一阶导数为零即可得到均值，求二阶导数得到方差，因此求导如下：
∂Lt∂xt−1=−AtTRt−1(xt−Atxt−1−Btut)+Σt−1−1(xt−1−μt−1)
\frac{\partial L_{t}}{\partial x_{t-1}}=-A_t^T R_t^{-1}(x_t-A_t x_{t-1}-B_t u_t) + \Sigma_{t-1}^{-1}(x_{t-1}-\mu_{t-1})
∂xt−1∂Lt=−AtTRt−1(xt−Atxt−1−Btut)+Σt−1−1(xt−1−μt−1)
∂2Lt∂xt−12=AtTRt−1At+Σt−1−1=:Ψt−1
\frac{\partial^2 L_{t}}{\partial x_{t-1}^2}=A_t^T R_t^{-1}A_t + \Sigma_{t-1}^{-1} =: \Psi _{t}^{-1}
∂xt−12∂2Lt=AtTRt−1At+Σt−1−1=:Ψt−1
令一阶导数为零有
AtTRt−1(xt−Atxt−1−Btut)=Σt−1−1(xt−1−μt−1)AtTRt−1xt−AtTRt−1Atxt−1−AtTRt−1Btut=Σt−1−1xt−1−Σt−1−1μt−1AtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1=Σt−1−1xt−1+AtTRt−1Atxt−1xt−1=AtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1Σt−1−1+AtTRt−1At
A_t^T R_t^{-1}(x_t-A_t x_{t-1}-B_t u_t) = \Sigma_{t-1}^{-1}(x_{t-1}-\mu_{t-1})\\
A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} A_t x_{t-1}-A_t^T R_t^{-1} B_t u_t = \Sigma_{t-1}^{-1}x_{t-1}-\Sigma_{t-1}^{-1} \mu_{t-1}\\
A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} B_t u_t +\Sigma_{t-1}^{-1} \mu_{t-1} = \Sigma_{t-1}^{-1}x_{t-1}+A_t^T R_t^{-1} A_t x_{t-1}\\
x_{t-1}= \frac{A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} B_t u_t +\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t}\\
AtTRt−1(xt−Atxt−1−Btut)=Σt−1−1(xt−1−μt−1)AtTRt−1xt−AtTRt−1Atxt−1−AtTRt−1Btut=Σt−1−1xt−1−Σt−1−1μt−1AtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1=Σt−1−1xt−1+AtTRt−1Atxt−1xt−1=Σt−1−1+AtTRt−1AtAtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1
定义二次型函数如下：
Lt(xt−1,xt)=12(xt−1−AtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1Σt−1−1+AtTRt−1At)TΨ−1(xt−1−AtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1Σt−1−1+AtTRt−1At)
L_t(x_{t-1},x_t)=\frac{1}{2}(x_{t-1}-\frac{A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} B_t u_t +\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t})^T\Psi^{-1}(x_{t-1}-\frac{A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} B_t u_t +\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t})
Lt(xt−1,xt)=21(xt−1−Σt−1−1+AtTRt−1AtAtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1)TΨ−1(xt−1−Σt−1−1+AtTRt−1AtAtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1)
有det(2πΨt)12∫exp{Lt(xt,xt−1)}dxt−1=1det(2\pi\Psi_t)^{\frac{1}{2}}\int exp\{L_t(x_t,x_{t-1})\}dx_{t-1}=1det(2πΨt)21∫exp{Lt(xt,xt−1)}dxt−1=1,因此只要将原式转换为Lt=Lt(xt,xt−1)+Lt(xt)L_t = L_t(x_t,x_{t-1}) + L_t(x_t)Lt=Lt(xt,xt−1)+Lt(xt)即可将积分去掉。
bel‾(xt)=η∫exp{−Lt}dxt−1=η∫exp{−Lt(xt)−Lt(xt,xt−1)}dxt−1=ηexp{−Lt(xt)}∫exp{−Lt(xt,xt−1)}dxt−1=ηexp{−Lt(xt)
\overline{bel}(x_t)=\eta \int exp\{-L_t \}dx_{t-1}\\
=\eta \int exp\{-L_t(x_t) -L_t(x_t,x_{t-1}) \}dx_{t-1}\\
=\eta exp\{-L_t(x_t) \}\int exp\{-L_t(x_t,x_{t-1}) \}dx_{t-1}\\
=\eta exp\{-L_t(x_t) \\
bel(xt)=η∫exp{−Lt}dxt−1=η∫exp{−Lt(xt)−Lt(xt,xt−1)}dxt−1=ηexp{−Lt(xt)}∫exp{−Lt(xt,xt−1)}dxt−1=ηexp{−Lt(xt)
因此只需要考虑Lt(xt)L_t(x_t)Lt(xt)的分布即可，由于对于xtx_txt是二次型，为高斯分布，可以采用求一阶导数和二阶导数的方法求得其均值和方差。
Lt(xt)=Lt−Lt(xt,xt−1)=12(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)+12(xt−1−μt−1)TΣt−1−1(xt−1−μt−1)−12(xt−1−AtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1Σt−1−1+AtTRt−1At)TΨ−1(xt−1−AtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1Σt−1−1+AtTRt−1At)
L_t(x_t) = L_t - L_t(x_t,x_{t-1})\\
=\frac{1}{2} (x_t-A_t x_{t-1}-B_t u_t)^TR_{t}^{-1}(x_t-A_t x_{t-1}-B_t u_t) + \frac{1}{2}(x_{t-1}-\mu_{t-1})^T \Sigma_{t-1}^{-1}(x_{t-1}-\mu_{t-1}) - \frac{1}{2}(x_{t-1}-\frac{A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} B_t u_t +\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t})^T\Psi^{-1}(x_{t-1}-\frac{A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} B_t u_t +\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t})
Lt(xt)=Lt−Lt(xt,xt−1)=21(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)+21(xt−1−μt−1)TΣt−1−1(xt−1−μt−1)−21(xt−1−Σt−1−1+AtTRt−1AtAtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1)TΨ−1(xt−1−Σt−1−1+AtTRt−1AtAtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1)
由于该式子的化简过于繁琐，就不化简直接求导数
一阶导数：
∂Lt(xt)∂xt=Rt−1(xt−Atxt−1−Btut)+Rt−1AtΣt−1−1+AtTRt−1AtΨ−1(xt−1−AtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1Σt−1−1+AtTRt−1At)=Rt−1(xt−Atxt−1−Btut)+Rt−1AtΣt−1−1+AtTRt−1At(AtTRt−1At+Σt−1−1)(xt−1−AtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1Σt−1−1+AtTRt−1At)=Rt−1(xt−Atxt−1−Btut)+Rt−1At(xt−1−AtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1Σt−1−1+AtTRt−1At)=Rt−1(xt−Btut)−Rt−1AtAtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1Σt−1−1+AtTRt−1At=Rt−1(xt−Btut)−Rt−1AtAtTRt−1(xt−Btut)+Σt−1−1μt−1Σt−1−1+AtTRt−1At=[Rt−1−Rt−1AtTAtRt−1Σt−1−1+AtTRt−1At](xt−Btut)−Rt−1AtΣt−1−1μt−1Σt−1−1+AtTRt−1At(求逆定理)=(Rt+AtΣt−1AtT)−1(xt−Btut)−Rt−1AtΣt−1−1μt−1Σt−1−1+AtTRt−1At\frac{\partial L_t(x_t)}{\partial x_t} = R_t^{-1} (x_t-A_t x_{t-1}-B_t u_t) + \frac{ R_t^{-1}A_t}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t} \Psi^{-1} (x_{t-1}-\frac{A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} B_t u_t +\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t})\\
=R_t^{-1} (x_t-A_t x_{t-1}-B_t u_t) + \frac{ R_t^{-1}A_t}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t} (A_t^T R_t^{-1}A_t + \Sigma_{t-1}^{-1}) (x_{t-1}-\frac{A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} B_t u_t +\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t})\\
=R_t^{-1} (x_t-A_t x_{t-1}-B_t u_t) + R_t^{-1} A_t (x_{t-1}-\frac{A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} B_t u_t +\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t})\\
=R_t^{-1} (x_t-B_t u_t) -R_t^{-1} A_t \frac{A_t^T R_t^{-1}x_t-A_t^T R_t^{-1} B_t u_t +\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t}\\
=R_t^{-1} (x_t-B_t u_t) -R_t^{-1} A_t \frac{A_t^T R_t^{-1}(x_t- B_t u_t) +\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t}\\
=[R_t^{-1} -R_t^{-1} A_t^T \frac{A_t R_t^{-1} }{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t}](x_t- B_t u_t)- R_t^{-1} A_t\frac{\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t}(求逆定理)\\
=(R_t + A_t \Sigma_{t-1}A_t^T)^{-1}(x_t- B_t u_t)- R_t^{-1} A_t \frac{\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t}\\
∂xt∂Lt(xt)=Rt−1(xt−Atxt−1−Btut)+Σt−1−1+AtTRt−1AtRt−1AtΨ−1(xt−1−Σt−1−1+AtTRt−1AtAtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1)=Rt−1(xt−Atxt−1−Btut)+Σt−1−1+AtTRt−1AtRt−1At(AtTRt−1At+Σt−1−1)(xt−1−Σt−1−1+AtTRt−1AtAtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1)=Rt−1(xt−Atxt−1−Btut)+Rt−1At(xt−1−Σt−1−1+AtTRt−1AtAtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1)=Rt−1(xt−Btut)−Rt−1AtΣt−1−1+AtTRt−1AtAtTRt−1xt−AtTRt−1Btut+Σt−1−1μt−1=Rt−1(xt−Btut)−Rt−1AtΣt−1−1+AtTRt−1AtAtTRt−1(xt−Btut)+Σt−1−1μt−1=[Rt−1−Rt−1AtTΣt−1−1+AtTRt−1AtAtRt−1](xt−Btut)−Rt−1AtΣt−1−1+AtTRt−1AtΣt−1−1μt−1(求逆定理)=(Rt+AtΣt−1AtT)−1(xt−Btut)−Rt−1AtΣt−1−1+AtTRt−1AtΣt−1−1μt−1
二阶导数：
∂2Lt(xt)∂xt2=(Rt+AtΣt−1AtT)−1
\frac{\partial^2 L_t(x_t)}{\partial x_t^2}=(R_t + A_t \Sigma_{t-1}A_t^T)^{-1}
∂xt2∂2Lt(xt)=(Rt+AtΣt−1AtT)−1
当一阶导数为零时
0=(Rt+AtΣt−1AtT)−1(xt−Btut)−Rt−1AtΣt−1−1μt−1Σt−1−1+AtTRt−1At(Rt+AtΣt−1AtT)−1(xt−Btut)=Rt−1AtΣt−1−1μt−1Σt−1−1+AtTRt−1At(xt−Btut)=(Rt+AtΣt−1AtT)Rt−1At(Σt−1−1+AtTRt−1At)−1Σt−1−1μt−1(xt−Btut)=(Rt+AtΣt−1AtT)Rt−1At(I+Σt−1AtTRt−1At)−1μt−1(xt−Btut)=(At+AtΣt−1AtTRt−1At)(I+Σt−1AtTRt−1At)−1μt−1(xt−Btut)=At(I+Σt−1AtTRt−1At)(I+Σt−1AtTRt−1At)−1μt−1(xt−Btut)=Atμt−1xt=Atμt−1+Btut
0=(R_t + A_t \Sigma_{t-1}A_t^T)^{-1}(x_t- B_t u_t)- R_t^{-1} A_t \frac{\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t}\\
(R_t + A_t \Sigma_{t-1}A_t^T)^{-1}(x_t- B_t u_t)= R_t^{-1} A_t \frac{\Sigma_{t-1}^{-1} \mu_{t-1}}{\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t}\\
(x_t- B_t u_t)= (R_t + A_t \Sigma_{t-1}A_t^T) R_t^{-1} A_t (\Sigma_{t-1}^{-1}+A_t^T R_t^{-1} A_t )^{-1}\Sigma_{t-1}^{-1} \mu_{t-1}\\
(x_t- B_t u_t)= (R_t + A_t \Sigma_{t-1}A_t^T) R_t^{-1} A_t(I+\Sigma_{t-1} A_t^T R_t^{-1} A_t )^{-1}\mu_{t-1}\\
(x_t- B_t u_t)= ( A_t + A_t \Sigma_{t-1}A_t^T R_t^{-1} A_t) (I+\Sigma_{t-1} A_t^T R_t^{-1} A_t )^{-1}\mu_{t-1}\\
(x_t- B_t u_t)= A_t ( I + \Sigma_{t-1}A_t^T R_t^{-1} A_t) (I+\Sigma_{t-1} A_t^T R_t^{-1} A_t )^{-1}\mu_{t-1}\\
(x_t- B_t u_t)= A_t \mu_{t-1}\\
x_t= A_t \mu_{t-1} + B_t u_t\\
0=(Rt+AtΣt−1AtT)−1(xt−Btut)−Rt−1AtΣt−1−1+AtTRt−1AtΣt−1−1μt−1(Rt+AtΣt−1AtT)−1(xt−Btut)=Rt−1AtΣt−1−1+AtTRt−1AtΣt−1−1μt−1(xt−Btut)=(Rt+AtΣt−1AtT)Rt−1At(Σt−1−1+AtTRt−1At)−1Σt−1−1μt−1(xt−Btut)=(Rt+AtΣt−1AtT)Rt−1At(I+Σt−1AtTRt−1At)−1μt−1(xt−Btut)=(At+AtΣt−1AtTRt−1At)(I+Σt−1AtTRt−1At)−1μt−1(xt−Btut)=At(I+Σt−1AtTRt−1At)(I+Σt−1AtTRt−1At)−1μt−1(xt−Btut)=Atμt−1xt=Atμt−1+Btut
后验
根据贝叶斯滤波器的后验公式
bel(xt)=ηp(zt∣xt)⏟∼N(zt:Ctxt,Qt)bel‾(xt)⏟∼N(xt:μˉt,Σˉt)
\operatorname{bel}\left(x_{t}\right)=\eta \underbrace{p\left(z_{t} | x_{t}\right)}_{\sim \mathcal{N}\left(z_{t}: C_{t} x_{t}, Q_{t}\right)} \underbrace{\overline{\operatorname{bel}}\left(x_{t}\right)}_{\sim \mathcal{N}\left(x_{t}: \bar{\mu}_{t}, \bar{\Sigma}_{t}\right)}
bel(xt)=η∼N(zt:Ctxt,Qt)p(zt∣xt)∼N(xt:μˉt,Σˉt)bel(xt)
简化表示为bel(xt)=ηexp{−Jt}bel(x_t)=\eta exp\{-J_t\}bel(xt)=ηexp{−Jt},其中
Jt=12(zt−Ctxt)TQ−1(zt−Ctxt)+12(xt−μ‾t)TΣ‾t−1(xt−μ‾t)
J_t=\frac{1}{2}(z_t-C_tx_t)^TQ^{-1}(z_t-C_tx_t) + \frac{1}{2}(x_t-\overline{\mu}_t)^T\overline{\Sigma}_t^{-1}(x_t-\overline{\mu}_t)
Jt=21(zt−Ctxt)TQ−1(zt−Ctxt)+21(xt−μt)TΣt−1(xt−μt)
由于依旧是xtx_txt的二次型，即满足高斯分布，同样可以采用求一阶导数和二阶导数的方法求均值和方差。
一阶导数：
∂J∂xt=−CtTQt−1(zt−Ctxt)+Σ‾t−1(xt−μ‾t)
\frac{\partial J}{\partial x_t} = -C_t^T Q_t^{-1}(z_t-C_tx_t) + \overline{\Sigma}_t^{-1}(x_t-\overline{\mu}_t)
∂xt∂J=−CtTQt−1(zt−Ctxt)+Σt−1(xt−μt)
二阶导数：
∂2J∂xt2=CtTQt−1Ct+Σ‾t−1 \frac{\partial^2 J}{\partial x_t^2} = C_t^T Q_t^{-1}C_t+ \overline{\Sigma}_t^{-1} ∂xt2∂2J=CtTQt−1Ct+Σt−1
因此，Σt=(CtTQt−1Ct+Σ‾t−1)−1\Sigma_t = (C_t^T Q_t^{-1}C_t+ \overline{\Sigma}_t^{-1})^{-1}Σt=(CtTQt−1Ct+Σt−1)−1
令一阶导数为零
0=−CtTQt−1(zt−Ctxt)+Σ‾t−1(xt−μ‾t)CtTQt−1(zt−Ctxt)=Σ‾t−1(xt−μ‾t)CtTQt−1zt−CtTQt−1Ctxt=Σ‾t−1xt−Σ‾t−1μ‾t(CtTQt−1Ct+Σ‾t−1)xt=CtTQt−1zt+Σ‾t−1μ‾txt=(CtTQt−1Ct+Σ‾t−1)−1CtTQt−1zt+(CtTQt−1Ct+Σ‾t−1)−1Σ‾t−1μ‾t
0 = -C_t^T Q_t^{-1}(z_t-C_tx_t) + \overline{\Sigma}_t^{-1}(x_t-\overline{\mu}_t)\\
C_t^T Q_t^{-1}(z_t-C_tx_t) = \overline{\Sigma}_t^{-1}(x_t-\overline{\mu}_t)\\
C_t^T Q_t^{-1}z_t-C_t^T Q_t^{-1} C_tx_t = \overline{\Sigma}_t^{-1}x_t-\overline{\Sigma}_t^{-1} \overline{\mu}_t\\
(C_t^T Q_t^{-1} C_t + \overline{\Sigma}_t^{-1})x_t = C_t^T Q_t^{-1}z_t + \overline{\Sigma}_t^{-1} \overline{\mu}_t\\
x_t = (C_t^T Q_t^{-1} C_t + \overline{\Sigma}_t^{-1})^{-1}C_t^T Q_t^{-1}z_t + (C_t^T Q_t^{-1} C_t + \overline{\Sigma}_t^{-1})^{-1} \overline{\Sigma}_t^{-1} \overline{\mu}_t\\
0=−CtTQt−1(zt−Ctxt)+Σt−1(xt−μt)CtTQt−1(zt−Ctxt)=Σt−1(xt−μt)CtTQt−1zt−CtTQt−1Ctxt=Σt−1xt−Σt−1μt(CtTQt−1Ct+Σt−1)xt=CtTQt−1zt+Σt−1μtxt=(CtTQt−1Ct+Σt−1)−1CtTQt−1zt+(CtTQt−1Ct+Σt−1)−1Σt−1μt
令xt=μ‾t+Kk(zt−Ctμ‾t)x_t = \overline{\mu}_t +K_k(z_t-C_t\overline{\mu}_t)xt=μt+Kk(zt−Ctμt),整理得
Kk=CtTQt−1(CtTQt−1Ct+Σ‾t−1)−1
K_k = C_t^T Q_t^{-1}(C_t^T Q_t^{-1} C_t + \overline{\Sigma}_t^{-1})^{-1}
Kk=CtTQt−1(CtTQt−1Ct+Σt−1)−1
最后将Σt\Sigma_tΣt也用KkK_kKk来表达，
Σt=(CtTQt−1Ct+Σ‾t−1)−1Σt=(I−CtTQt−1(CtTQt−1Ct+Σ‾t−1)−1Ct)Σ‾tΣt=(I−KkCt)Σ‾t
\Sigma_t = (C_t^T Q_t^{-1}C_t+ \overline{\Sigma}_t^{-1})^{-1}\\
\Sigma_t = (I - C_t^T Q_t^{-1}(C_t^T Q_t^{-1} C_t + \overline{\Sigma}_t^{-1})^{-1}C_t )\overline{\Sigma}_t\\
\Sigma_t = (I - K_kC_t )\overline{\Sigma}_t\\
Σt=(CtTQt−1Ct+Σt−1)−1Σt=(I−CtTQt−1(CtTQt−1Ct+Σt−1)−1Ct)ΣtΣt=(I−KkCt)Σt
综上，卡尔曼滤波器的五个公式便推导完毕了。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
