文章目录

公式与符号说明
几个性质及部分证明
高斯白噪声中的直流电平
卡尔曼滤波正式推导
参考

本文从联合正态分布的一些性质出发推导标量和向量形式的卡尔曼滤波，以及重点推导了均值不为0的各个公式。下面的一些性质出于方便考虑给起了名字，不一定是正式的学术命名。
正式证明还没写完。

公式与符号说明

离散系统
x(k)=Ax(k−1)+Bu(k−1)+v(k−1)y(k)=Cx(k)+W(k)\begin{aligned} & x(k)=Ax(k-1)+Bu(k-1)+v(k-1) \\ & y(k)=Cx(k)+W(k) \end{aligned} x(k)=Ax(k−1)+Bu(k−1)+v(k−1)y(k)=Cx(k)+W(k)
其中V,WV,WV,W为零均值高斯白噪声的协方差矩阵。
卡尔曼滤波器递推公式如下
x^(k∣k−1)=Ax^(k−1)+Bu(k−1)x^(k)=x^(k∣k−1)+K(k)[y(k)−Cx^(k∣k−1)]P(k∣k−1)=AP(k−1)AT+VP(k)=[I−K(k)C]P(k∣k−1)K(k)=P(k∣k−1)CT[CP(k∣k−1)CT+W]−1\begin{aligned} & \hat{x}(k|k-1)=A\hat{x}(k-1)+Bu(k-1) \\ & \hat{x}(k)=\hat{x}(k|k-1)+K(k)[y(k)-C\hat{x}(k|k-1)] \\ & P(k|k-1)=AP(k-1)A^{\text{T}}+V \\ & P(k)=[I-K(k)C]P(k|k-1) \\ & K(k)=P(k|k-1)C^{\text{T}}[CP(k|k-1)C^{\text{T}}+W]^{-1} \\ \end{aligned} x^(k∣k−1)=Ax^(k−1)+Bu(k−1)x^(k)=x^(k∣k−1)+K(k)[y(k)−Cx^(k∣k−1)]P(k∣k−1)=AP(k−1)AT+VP(k)=[I−K(k)C]P(k∣k−1)K(k)=P(k∣k−1)CT[CP(k∣k−1)CT+W]−1

Y(k)=[y(0),y(1),⋯,y(k)]\mathbf{Y}(k)=[y(0),y(1),\cdots,y(k)]Y(k)=[y(0),y(1),⋯,y(k)]表示前kkk个时刻的观测数据
x^(k)=E[x(k)∣Y(k)]\hat{x}(k)=\text{E}[x(k)|Y(k)]x^(k)=E[x(k)∣Y(k)]表示根据前kkk个时刻的观测数据对第kkk时刻的实际状态x(k)x(k)x(k)的预测值
x^(k∣k−1)=E[x(k)∣Y(k−1)]\hat{x}(k|k-1)=\text{E}[x(k)|Y(k-1)]x^(k∣k−1)=E[x(k)∣Y(k−1)]表示根据前k−1k-1k−1个时刻的观测数据对第kkk时刻的实际状态x(k)x(k)x(k)的预测值
P(k∣k−1)=D[x(k)−x^(k∣k−1)]P(k|k-1)=\text{D}[x(k)-\hat{x}(k|k-1)]P(k∣k−1)=D[x(k)−x^(k∣k−1)]称预测方差，或预测协方差矩阵
P(k)=D[x(k)−x^(k)]P(k)=\text{D}[x(k)-\hat{x}(k)]P(k)=D[x(k)−x^(k)]称估计方差，或估计协方差矩阵
V=Dv(k)V=Dv(k)V=Dv(k)为状态噪声方差或协方差矩阵
W=Dw(k)W=Dw(k)W=Dw(k)为观测噪声方差或协方差矩阵

几个性质及部分证明

贝叶斯最小均方误差估计量(Bmse)
观测到xxx后θ\thetaθ的最小均方误差估计量θ^\hat{\theta}θ^为
θ^=E(θ∣x)\hat{\theta}=\text{E}(\theta|x)θ^=E(θ∣x)
证明的主要思路是求θ^\hat{\theta}θ^使得最小均方误差最小。证明：
Bmse(θ^)=E[(θ−θ^)2]=∬(θ−θ^)2p(x,θ)dxdθ=∫[∫(θ−θ^)2p(θ∣x)dθ]p(x)dxJ=∫(θ−θ^)2p(θ∣x)dθ∂J∂θ^=−2∫θp(θ∣x)dθ+2θ^∫p(θ∣x)dθ=0θ^=2∫θp(θ∣x)dθ2∫p(θ∣x)dθ=E(θ∣x)\begin{aligned} \text{Bmse}(\hat{\theta}) &= \text{E}[(\theta-\hat{\theta})^2] \\ &= \iint(\theta-\hat{\theta})^2p(x,\theta)\text{d}x\text{d}\theta \\ &=\int\left[\int(\theta-\hat{\theta})^2p(\theta|x)\text{d}\theta\right] p(x)\text{d}x \\ J &= \int(\theta-\hat{\theta})^2p(\theta|x)\text{d}\theta \\ \frac{\partial J}{\partial\hat{\theta}} &= -2\int\theta p(\theta|x)\text{d}\theta +2\hat{\theta}\int p(\theta|x)\text{d}\theta=0 \\ \hat{\theta} &= \frac{2\displaystyle\int\theta p(\theta|x)\text{d}\theta} {2\displaystyle\int p(\theta|x)\text{d}\theta}=\text{E}(\theta|x) \\ \end{aligned} Bmse(θ^)J∂θ^∂Jθ^=E[(θ−θ^)2]=∬(θ−θ^)2p(x,θ)dxdθ=∫[∫(θ−θ^)2p(θ∣x)dθ]p(x)dx=∫(θ−θ^)2p(θ∣x)dθ=−2∫θp(θ∣x)dθ+2θ^∫p(θ∣x)dθ=0=2∫p(θ∣x)dθ2∫θp(θ∣x)dθ=E(θ∣x)
零均值应用定理(标量形式)
设xxx和yyy为联合正态分布的随机变量，则
E(y∣x)=Ey+Cov(x,y)Dx(x−Ex)D(y∣x)=Dy−Cov2(x,y)Dx\begin{aligned} & \text{E}(y|x)=\text{E}y+\frac{\text{Cov}(x,y)}{\text{D}x}(x-\text{E}x) \\ & \text{D}(y|x)=\text{D}y-\frac{\text{Cov}^2(x,y)}{\text{D}x} \\ \end{aligned} E(y∣x)=Ey+DxCov(x,y)(x−Ex)D(y∣x)=Dy−DxCov2(x,y)
两式可另外写作
y^−EyDy=ρx−ExDxD(y∣x)=Dy(1−ρ2)\begin{aligned} & \frac{\hat{y}-\text{E}y}{\sqrt{\text{D}y}} =\rho\frac{x-\text{E}x}{\sqrt{\text{D}x}} \\ & \text{D}(y|x)=\text{D}y(1-\rho^2) \\ \end{aligned} Dyy^−Ey=ρDxx−ExD(y∣x)=Dy(1−ρ2)
证明：
y^=ax+bJ=E(y−y^)2=E[y2−2y(ax+b)+(ax+b)2]=y2−2Exy⋅a−2bEy+Ex2⋅a2+2Ex⋅ab+b2dJ=(−2Exy+2aEx2+2bEx)da+(−2Ey+2aEx+2b)db∂J∂b=−2Ey+2aEx+2b=0b=Ey−aEx∂J∂a=−2Exy+2aEx2+2bEx=0aEx2=Exy−bEx=Exy−ExEy+a(Ex)2a=Cov(x,y)Dxy^=ax+b=ax+Ey−aEx=Ey+Cov(x,y)Dx(x−Ex)\begin{aligned} \hat{y} &= ax+b \\ J &= \text{E}(y-\hat{y})^2 \\ &= \text{E}[y^2-2y(ax+b)+(ax+b)^2] \\ &= y^2-2\text{E}xy\cdot a-2b\text{E}y +\text{E}x^2\cdot a^2+2\text{E}x\cdot ab+b^2 \\ \text{d}J &= (-2\text{E}xy+2a\text{E}x^2+2b\text{E}x)\text{d}a +(-2\text{E}y+2a\text{E}x+2b)\text{d}b \\ \frac{\partial J}{\partial b} &= -2\text{E}y+2a\text{E}x+2b = 0 \\ b &= \text{E}y-a\text{E}x \\ \frac{\partial J}{\partial a} &= -2\text{E}xy+2a\text{E}x^2+2b\text{E}x = 0 \\ a\text{E}x^2 &= \text{E}xy-b\text{E}x = \text{E}xy-\text{E}x\text{E}y+a(\text{E}x)^2 \\ a &= \frac{\text{Cov}(x,y)}{\text{D}x} \\ \hat{y} &= ax+b = ax+\text{E}y-a\text{E}x = \text{E}y+\frac{\text{Cov}(x,y)}{\text{D}x}(x-\text{E}x) \\ \end{aligned} y^JdJ∂b∂Jb∂a∂JaEx2ay^=ax+b=E(y−y^)2=E[y2−2y(ax+b)+(ax+b)2]=y2−2Exy⋅a−2bEy+Ex2⋅a2+2Ex⋅ab+b2=(−2Exy+2aEx2+2bEx)da+(−2Ey+2aEx+2b)db=−2Ey+2aEx+2b=0=Ey−aEx=−2Exy+2aEx2+2bEx=0=Exy−bEx=Exy−ExEy+a(Ex)2=DxCov(x,y)=ax+b=ax+Ey−aEx=Ey+DxCov(x,y)(x−Ex)
该定理只对包括正态分布在内的满足线性关系的随机变量有效(具体什么地方满足线性暂时没搞清楚)。例如，对两个联合均匀分布
f(x,y)=2,0<x<1,0<y<xf(x,y)=3,0<x<1,x2<y<x\begin{aligned} & f(x,y)=2,\quad 0<x<1,0<y<x \\ & f(x,y)=3,\quad 0<x<1,x^2<y<\sqrt{x} \end{aligned} f(x,y)=2,0<x<1,0<y<xf(x,y)=3,0<x<1,x2<y<x
第一个成立，第二个由于非线性的存在而不成立，也就是说yyy在数据xxx下的线性贝叶斯估计量不是最佳估计量，线性贝叶斯估计量和最佳贝叶斯估计量分别为
y^=Ey+Cov(x,y)Dx(x−Ex)=133x+9153y^=E(y∣x)=x+x22\begin{aligned} & \hat{y}=\text{E}y+\frac{\text{Cov}(x,y)}{\text{D}x}(x-\text{E}x) =\frac{133x+9}{153} \\ & \hat{y}=\text{E}(y|x)=\frac{\sqrt{x}+x^2}{2} \\ \end{aligned} y^=Ey+DxCov(x,y)(x−Ex)=153133x+9y^=E(y∣x)=2x+x2
零均值应用定理(向量形式)
x\boldsymbol{x}x和y\boldsymbol{y}y为联合正态分布的随机向量，x\boldsymbol{x}x是m×1，y\boldsymbol{y}y是n×1，分块协方差矩阵
C=[CxxCxyCyxCyy]\mathbf{C}=\left[\begin{matrix} \mathbf{C}_{xx} & \mathbf{C}_{xy} \\ \mathbf{C}_{yx} & \mathbf{C}_{yy} \end{matrix}\right] C=[CxxCyxCxyCyy]
则
E(y∣x)=E(y)+CyxCxx−1(x−E(x))\text{E}(\boldsymbol{y}|\boldsymbol{x})=\text{E}(\boldsymbol{y}) +\mathbf{C}_{yx}\mathbf{C}_{xx}^{-1}(\boldsymbol{x}-\text{E}(\boldsymbol{x})) E(y∣x)=E(y)+CyxCxx−1(x−E(x))
其中CxyC_{xy}Cxy表示Cov(x,y)\text{Cov}(x,y)Cov(x,y)。证明：
(其中省略的步骤见下文)
y^=Ax+BJ=E(y−y^)⊤(y−y^)=E(y⊤y−2y⊤y^+y^⊤y^)K=y⊤y−2y⊤y^+y^⊤y^dK=d(−2y⊤(Ax+B)+(Ax+B)⊤(Ax+B))=−2y⊤(dAx+dB)+d(x⊤A⊤Ax+2B⊤Ax+B⊤B)=−2xy⊤dA−2y⊤dB+2xx⊤A⊤dA+2x⊤A⊤dB+2xB⊤dA+2B⊤dB=(−2xy⊤+2xx⊤A⊤+2xB⊤)dA+(−2y⊤+2x⊤A⊤+2B⊤)dB∂J∂B=−2y+2Ax+2B=0B=Ey−AEx∂J∂A=−2yx⊤+2Axx⊤+2Bx⊤=0E(Axx⊤)=E(yx⊤−Bx⊤)=E(yx⊤−(Ey−AEx)x⊤)=Eyx⊤−EyEx⊤+AExEx⊤A=(Eyx⊤−EyEx⊤)(E(xx⊤)−ExEx⊤)−1=CyxCxx−1y^=CyxCxx−1x+Ey−CyxCxx−1Ex=Ey+CyxCxx−1(x−Ex)\begin{aligned} \hat{y} &= Ax+B \\ J &= \text{E}(y-\hat{y})^\top(y-\hat{y}) \\ &= \text{E}(y^\top y-2y^\top\hat{y}+\hat{y}^\top\hat{y}) \\ K &= y^\top y-2y^\top\hat{y}+\hat{y}^\top\hat{y} \\ \text{d}K &= \text{d}(-2y^\top(Ax+B)+(Ax+B)^\top(Ax+B)) \\ &= -2y^\top(\text{d}Ax+\text{d}B) +\text{d}(x^\top A^\top Ax+2B^\top Ax+B^\top B) \\ &= -2xy^\top\text{d}A-2y^\top\text{d}B+2xx^\top A^\top\text{d}A \\ &+ 2x^\top A^\top\text{d}B+2xB^\top\text{d}A+2B^\top\text{d}B \\ &= (-2xy^\top+2xx^\top A^\top+2xB^\top)\text{d}A +(-2y^\top+2x^\top A^\top+2B^\top)\text{d}B \\ \frac{\partial J}{\partial B} &= -2y+2Ax+2B =0\\ B &= \text{E}y-A\text{E}x \\ \frac{\partial J}{\partial A} &= -2yx^\top+2Axx^\top+2Bx^\top =0\\ \text{E}(Axx^\top) &= \text{E}(yx^\top-Bx^\top) \\ &= \text{E}(yx^\top-(\text{E}y-A\text{E}x)x^\top) \\ &= \text{E}yx^\top-\text{E}y\text{E}x^\top+A\text{E}x\text{E}x^\top \\ A &= (\text{E}yx^\top-\text{E}y\text{E}x^\top) (\text{E}(xx^\top)-\text{E}x\text{E}x^\top)^{-1} =C_{yx}C_{xx}^{-1} \\ \hat{y} &= C_{yx}C_{xx}^{-1}x+\text{E}y-C_{yx}C_{xx}^{-1}\text{E}x \\ &= \text{E}y+C_{yx}C_{xx}^{-1}(x-\text{E}x) \\ \end{aligned} y^JKdK∂B∂JB∂A∂JE(Axx⊤)Ay^=Ax+B=E(y−y^)⊤(y−y^)=E(y⊤y−2y⊤y^+y^⊤y^)=y⊤y−2y⊤y^+y^⊤y^=d(−2y⊤(Ax+B)+(Ax+B)⊤(Ax+B))=−2y⊤(dAx+dB)+d(x⊤A⊤Ax+2B⊤Ax+B⊤B)=−2xy⊤dA−2y⊤dB+2xx⊤A⊤dA+2x⊤A⊤dB+2xB⊤dA+2B⊤dB=(−2xy⊤+2xx⊤A⊤+2xB⊤)dA+(−2y⊤+2x⊤A⊤+2B⊤)dB=−2y+2Ax+2B=0=Ey−AEx=−2yx⊤+2Axx⊤+2Bx⊤=0=E(yx⊤−Bx⊤)=E(yx⊤−(Ey−AEx)x⊤)=Eyx⊤−EyEx⊤+AExEx⊤=(Eyx⊤−EyEx⊤)(E(xx⊤)−ExEx⊤)−1=CyxCxx−1=CyxCxx−1x+Ey−CyxCxx−1Ex=Ey+CyxCxx−1(x−Ex)
其中矩阵求导的部分见矩阵求导术(上)，其中的推导过程省略了迹的符号tr\text{tr}tr，注意分辨。另外因为3个符号d\text{d}d、E\text{E}E、tr\text{tr}tr均为线性算符，因此可交换计算顺序，推导中也省略了E\text{E}E。
上面推导中用到的一些矩阵微分与求迹公式详细推导如下：
d(B⊤B)=tr(dB⊤B+B⊤dB)=tr(dB⊤B)+tr(B⊤dB)=tr(B⊤dB)+tr(B⊤dB)=tr(2B⊤dB)d(x⊤A⊤Ax)=tr(x⊤d(A⊤A)x)=tr(x⊤2A⊤dAx)=tr(2xx⊤A⊤dA)d(B⊤Ax)=tr(dB⊤Ax+B⊤dAx)=tr(x⊤A⊤dB+xB⊤dA)\begin{aligned} \text{d}(B^\top B) &= \text{tr}(\text{d}B^\top B+B^\top\text{d}B) \\ &= \text{tr}(\text{d}B^\top B)+\text{tr}(B^\top\text{d}B) \\ &= \text{tr}(B^\top\text{d}B)+\text{tr}(B^\top\text{d}B) \\ &= \text{tr}(2B^\top\text{d}B) \\ \text{d}(x^\top A^\top Ax) &= \text{tr}(x^\top\text{d}(A^\top A)x) \\ &= \text{tr}(x^\top2A^\top\text{d}Ax) \\ &= \text{tr}(2xx^\top A^\top\text{d}A) \\ \text{d}(B^\top Ax) &= \text{tr}(\text{d}B^\top Ax+B^\top\text{d}Ax) \\ &= \text{tr}(x^\top A^\top\text{d}B+xB^\top\text{d}A) \\ \end{aligned} d(B⊤B)d(x⊤A⊤Ax)d(B⊤Ax)=tr(dB⊤B+B⊤dB)=tr(dB⊤B)+tr(B⊤dB)=tr(B⊤dB)+tr(B⊤dB)=tr(2B⊤dB)=tr(x⊤d(A⊤A)x)=tr(x⊤2A⊤dAx)=tr(2xx⊤A⊤dA)=tr(dB⊤Ax+B⊤dAx)=tr(x⊤A⊤dB+xB⊤dA)

投影定理(正交原理)

当利用数据样本的线性组合来估计一个随机变量的时候，当估计值与真实值的误差和每一个数据样本正交时，该估计值是最佳估计量，即数据样本xxx与最佳估计量y^\hat{y}y^满足
E[(y−y^)x⊤(n)]=0n=0,1,⋯,N−1\text{E}[(y-\hat{y})x^\top(n)]=0\quad n=0,1,\cdots,N-1 E[(y−y^)x⊤(n)]=0n=0,1,⋯,N−1
零均值的随机变量满足内积空间中的性质。定义变量的长度∣∣x∣∣=Ex2||x||=\sqrt{\text{E}x^2}∣∣x∣∣=Ex2，变量xxx和yyy的内积(x,y)(x,y)(x,y)定义为E(xy)\text{E}(xy)E(xy)，两个变量的夹角定义为相关系数ρ\rhoρ。当E(xy)=0\text{E}(xy)=0E(xy)=0时称变量xxx和yyy正交。
均值不为零时，定义变量的长度∣∣x∣∣=Dx||x||=\sqrt{\text{D}x}∣∣x∣∣=Dx，变量xxx和yyy的内积(x,y)(x,y)(x,y)定义为Cov(xy)\text{Cov}(xy)Cov(xy)，两个变量的夹角定义为相关系数ρ\rhoρ。均值不为零的情况是我自己的猜测，很多资料都没有详细说明，但卡尔曼滤波的推导里全是均值非零的。
将xxx和yyy对应成数据的形式，即x(0)x(0)x(0)是xxx，x(1)x(1)x(1)是yyy，x^(1∣0)\hat{x}(1|0)x^(1∣0)是y^\hat{y}y^，得到
E[(x(1)−x^(1∣0))⊤x(0)]=0\text{E}[(x(1)-\hat{x}(1|0))^\top x(0)]=0 E[(x(1)−x^(1∣0))⊤x(0)]=0
其中
x~(k∣k−1)=x(k)−x^(k∣k−1)\widetilde{x}(k|k-1) = x(k)-\hat{x}(k|k-1) x(k∣k−1)=x(k)−x^(k∣k−1)
称为新息(innovation)，与旧数据x(0)x(0)x(0)正交。
标量形式证明：
E[x(y^−y)]=E[xEy+Cov(x,y)Dx(x2−xEx)−xy]=ExEy+Cov(x,y)Dx(Ex2−(Ex)2)−Exy=Cov(x,y)+ExEy−Exy=0\begin{aligned} \text{E}[x(\hat{y}-y)] &= \text{E}[x\text{E}y +\frac{\text{Cov}(x,y)}{\text{D}x}(x^2-x\text{E}x)-xy] \\ &= \text{E}x\text{E}y +\frac{\text{Cov}(x,y)}{\text{D}x}(\text{E}x^2-(\text{E}x)^2)-\text{E}xy \\ &= \text{Cov}(x,y)+\text{E}x\text{E}y-\text{E}xy=0 \end{aligned} E[x(y^−y)]=E[xEy+DxCov(x,y)(x2−xEx)−xy]=ExEy+DxCov(x,y)(Ex2−(Ex)2)−Exy=Cov(x,y)+ExEy−Exy=0
向量形式证明：
E[(y^−y)x⊤]=E[Eyx⊤+CyxCxx−1(x−Ex)x⊤−yx⊤]=EyEx⊤+E[CyxCxx−1xx⊤]−CyxCxx−1ExEx⊤−Eyx⊤=−Cyx+CyxCxx−1(Exx⊤−ExEx⊤)=−Cyx+CyxCxx−1Cxx=0\begin{aligned} \text{E}[(\hat{y}-y)x^\top] &= \text{E}[\text{E}yx^\top+C_{yx}C_{xx}^{-1}(x-\text{E}x)x^\top-yx^\top] \\ &= \text{E}y\text{E}x^\top+\text{E}[C_{yx}C_{xx}^{-1}xx^\top] -C_{yx}C_{xx}^{-1}\text{E}x\text{E}x^\top-\text{E}yx^\top \\ &= -C_{yx}+C_{yx}C_{xx}^{-1}(\text{E}xx^\top-\text{E}x\text{E}x^\top) \\ &= -C_{yx}+C_{yx}C_{xx}^{-1}C_{xx} \\ &= 0 \end{aligned} E[(y^−y)x⊤]=E[Eyx⊤+CyxCxx−1(x−Ex)x⊤−yx⊤]=EyEx⊤+E[CyxCxx−1xx⊤]−CyxCxx−1ExEx⊤−Eyx⊤=−Cyx+CyxCxx−1(Exx⊤−ExEx⊤)=−Cyx+CyxCxx−1Cxx=0
由于E(y−y^)=0\text{E}(y-\hat{y})=0E(y−y^)=0，所以均值非零时正交条件也恰好成立。由图可得投影定理的另一个公式
E[(y−y^)y^]=0\text{E}[(y-\hat{y})\hat{y}]=0 E[(y−y^)y^]=0
证明：
E[y^(y^−y)]=E[(Ey+kx−kEx)(Ey+kx−kEx−y)]=(Ey)2+kExEy−kExEy−(Ey)2+kExEy+k2Ex2−k2(Ex)2−kExy−kExEy−k2(Ex)2+k2(Ex)2+kExEy=k2Dx−kCov(x,y)=[Cov(x,y)]2[Dx]2Dx−Cov(x,y)DxCov(x,y)=0\begin{aligned} & \text{E}[\hat{y}(\hat{y}-y)] \\ =& \text{E}[(\text{E} y+k x-k \text{E} x)(\text{E} y+k x-k \text{E} x-y)] \\ =& (\text{E}y)^{2}+k\text{E}x\text{E}y-k\text{E}x\text{E}y-(\text{E}y)^{2}\\ &+ k\text{E}x\text{E}y+k^{2}Ex^2-k^{2}(\text{E}x)^{2}-k\text{E}xy \\ &- k\text{E}x\text{E}y-k^{2}(\text{E}x)^{2}+k^{2}(\text{E}x)^{2}+k\text{E}x\text{E}y \\ =& k^{2}\text{D}x-k\text{Cov}(x,y) \\ =& \frac{[\text{Cov}(x,y)]^2}{[\text{D}x]^2}\text{D}x-\frac{\text{Cov}(x,y)}{\text{D}x}\text{Cov}(x,y) \\ =&0 \end{aligned} =====E[y^(y^−y)]E[(Ey+kx−kEx)(Ey+kx−kEx−y)](Ey)2+kExEy−kExEy−(Ey)2+kExEy+k2Ex2−k2(Ex)2−kExy−kExEy−k2(Ex)2+k2(Ex)2+kExEyk2Dx−kCov(x,y)[Dx]2[Cov(x,y)]2Dx−DxCov(x,y)Cov(x,y)0
期望可加性
E[y1+y2∣x]=E[y1∣x]+E[y2∣x]\text{E}[y_1+y_2|x]=\text{E}[y_1|x]+\text{E}[y_2|x]E[y1+y2∣x]=E[y1∣x]+E[y2∣x]
独立条件可加性
若x1x_1x1和x2x_2x2独立，则
E[y∣x1,x2]=E[y∣x1]+E[y∣x2]−Ey\text{E}[y|x_1,x_2]=\text{E}[y|x_1]+\text{E}[y|x_2]-\text{E}y E[y∣x1,x2]=E[y∣x1]+E[y∣x2]−Ey
证明：
令x=[x1⊤,x2⊤]⊤x=[x_1^\top,x_2^\top]^\topx=[x1⊤,x2⊤]⊤，则
Cxx−1=[Cx1x1Cx1x2Cx2x1Cx2x2]−1=[Cx1x1−1OOCx2x2−1]Cyx=[Cyx1Cyx2]E(y∣x)=Ey+CyxCxx−1(x−Ex)=Ey+[Cyx1Cyx2][Cx1x1−1OOCx2x2−1][x1−Ex1x2−Ex2]=E[y∣x1]+E[y∣x2]−Ey\begin{aligned} C_{xx}^{-1} &= \left[\begin{matrix} C_{x_1x_1} & C_{x_1x_2} \\ C_{x_2x_1} & C_{x_2x_2} \end{matrix}\right]^{-1} = \left[\begin{matrix} C_{x_1x_1}^{-1} & O \\ O & C_{x_2x_2}^{-1} \end{matrix}\right] \\ C_{yx} &= \left[\begin{matrix} C_{yx_1} & C_{yx_2} \end{matrix}\right] \\ \text{E}(y|x) &= \text{E}y+C_{yx}C_{xx}^{-1}(x-\text{E}x) \\ &= \text{E}y+\left[\begin{matrix} C_{yx_1} & C_{yx_2} \end{matrix}\right] \left[\begin{matrix} C_{x_1x_1}^{-1} & O \\ O & C_{x_2x_2}^{-1} \end{matrix}\right] \left[\begin{matrix} x_1-\text{E}x_1 \\ x_2-\text{E}x_2 \end{matrix}\right] \\ &= \text{E}[y|x_1]+\text{E}[y|x_2]-\text{E}y \end{aligned} Cxx−1CyxE(y∣x)=[Cx1x1Cx2x1Cx1x2Cx2x2]−1=[Cx1x1−1OOCx2x2−1]=[Cyx1Cyx2]=Ey+CyxCxx−1(x−Ex)=Ey+[Cyx1Cyx2][Cx1x1−1OOCx2x2−1][x1−Ex1x2−Ex2]=E[y∣x1]+E[y∣x2]−Ey
非独立条件可加性(新息定理)
若x1x_1x1和x2x_2x2不独立，则根据投影定理取x2x_2x2与x1x_1x1独立的分量x~2\widetilde{x}_2x2，满足
E[y∣x1,x2]=E[y∣x1,x~2]=E[y∣x1]+E[y∣x~2]−Ey\text{E}[y|x_1,x_2] =\text{E}[y|x_1,\widetilde{x}_2] =\text{E}[y|x_1]+\text{E}[y|\widetilde{x}_2]-\text{E}y E[y∣x1,x2]=E[y∣x1,x2]=E[y∣x1]+E[y∣x2]−Ey
其中x~2=x2−x^2=x2−E(x2∣x1)\widetilde{x}_2=x_2-\hat{x}_2=x_2-\text{E}(x_2|x_1)x2=x2−x^2=x2−E(x2∣x1)，由投影定理，x1x_1x1与x~2\widetilde{x}_2x2独立，x~2\widetilde{x}_2x2称为新息。
证明：
(下面的每个式子是先求部分后求整体，为便于理解可以从下往上看)
(标量情况下，Cx1x2=Cx2x1C_{x_1x_2}=C_{x_2x_1}Cx1x2=Cx2x1)
Cov(y,x^2)=E[y(Ex2+Cov(x2,x1)Dx1(x1−Ex1))]−EyEx^2=EyEx2+Cov(x2,x1)Dx1(Ex1y−Ex1Ey)−EyEx2=Cx1x2Cyx1Dx1Cov(x2,x^2)=Ex2x^2−Ex2Ex^2=E[x2(Ex2+Cov(x2,x1)Dx1(x1−Ex1))]−(Ex2)2=Cov(x2,x1)Dx1(Ex2x1−Ex2Ex1)=Cx1x22Dx1Dx^2=D(Ex2+Cov(x2,x1)Dx1(x1−Ex1))=Cx1x22(Dx1)2Dx1=Cx1x22Dx1Dx~2=Dx2+Dx^2−2Cov(x2,x^2)=Dx2−Cx1x22Dx1E[y∣x1]+E[y∣x~2]−Ey=Ey+Cx1yDx1(x1−Ex1)+Cov(y,x~2)Dx~2(x~2−Ex~2)=OMIT+Cov(y,x2)−Cov(y,x^2)Dx~2(x2−x^2)=OMIT+Cyx2−Cx1x2Cyx1Dx1Dx2−Cx1x22Dx1(x2−E2−Cov(x2,x1)Dx1(x1−E1))=OMIT+Cyx2Dx1−Cx1x2Cyx1Dx1Dx2−Cx1x22(x2−E2−Cx1x2Dx1(x1−E1))=Ey+A(x1−Ex1)+B(x2−E2)\begin{aligned} \text{Cov}(y,\hat{x}_2) &= \text{E}[y\left(\text{E}x_2 +\frac{\text{Cov}(x_2,x_1)}{\text{D}x_1}(x_1-\text{E}x_1)\right)] -\text{E}y\text{E}\hat{x}_2 \\ &= \text{E}y\text{E}x_2+\frac{\text{Cov}(x_2,x_1)}{\text{D}x_1} (\text{E}x_1y-\text{E}x_1\text{E}y)-\text{E}y\text{E}x_2 \\ &= \frac{C_{x_1x_2}C_{yx_1}}{\text{D}x_1} \\ \text{Cov}(x_2,\hat{x}_2) &= \text{E}x_2\hat{x}_2-\text{E}x_2\text{E}\hat{x}_2 \\ &= \text{E}[x_2(\text{E}x_2+\frac{\text{Cov}(x_2,x_1)}{\text{D}x_1} (x_1-\text{E}x_1))]-(\text{E}x_2)^2 \\ &= \frac{\text{Cov}(x_2,x_1)}{\text{D}x_1}(\text{E}x_2x_1-\text{E}x_2\text{E}x_1) \\ &= \frac{C_{x_1x_2}^2}{\text{D}x_1} \\ \text{D}\hat{x}_2 &= \text{D}(\text{E}x_2+\frac{\text{Cov}(x_2,x_1)}{\text{D}x_1} (x_1-\text{E}x_1)) \\ &= \frac{C_{x_1x_2}^2}{(\text{D}x_1)^2}\text{D}x_1 =\frac{C_{x_1x_2}^2}{\text{D}x_1} \\ \text{D}\widetilde{x}_2 &= \text{D}x_2+\text{D}\hat{x}_2 -2\text{Cov}(x_2,\hat{x}_2) \\ &= \text{D}x_2-\frac{C_{x_1x_2}^2}{\text{D}x_1} \\ \text{E}[y|x_1]+\text{E}[y|\widetilde{x}_2]-\text{E}y &= \text{E}y +\frac{C_{x_1y}}{\text{D}x_1}(x_1-\text{E}x_1) +\frac{\text{Cov}(y,\widetilde{x}_2)}{\text{D}\widetilde{x}_2} (\widetilde{x}_2-\text{E}\widetilde{x}_2) \\ &= \text{OMIT}+\frac{\text{Cov}(y,x_2)-\text{Cov}(y,\hat{x}_2)} {\text{D}\widetilde{x}_2}(x_2-\hat{x}_2) \\ &= \text{OMIT}+\frac{C_{yx_2}-\displaystyle\frac{C_{x_1x_2}C_{yx_1}}{\text{D}x_1}} {\text{D}x_2-\displaystyle\frac{C_{x_1x_2}^2}{\text{D}x_1}}(x_2-\text{E}_2 -\frac{\text{Cov}(x_2,x_1)}{\text{D}x_1}(x_1-\text{E}_1)) \\ &= \text{OMIT}+\frac{C_{yx_2}\text{D}x_1-C_{x_1x_2}C_{yx_1}} {\text{D}x_1\text{D}x_2-C_{x_1x_2}^2}(x_2-\text{E}_2 -\frac{C_{x_1x_2}}{\text{D}x_1}(x_1-\text{E}_1)) \\ &= \text{E}y+A(x_1-\text{E}x_1) +B(x_2-\text{E}_2) \\ \end{aligned} Cov(y,x^2)Cov(x2,x^2)Dx^2Dx2E[y∣x1]+E[y∣x2]−Ey=E[y(Ex2+Dx1Cov(x2,x1)(x1−Ex1))]−EyEx^2=EyEx2+Dx1Cov(x2,x1)(Ex1y−Ex1Ey)−EyEx2=Dx1Cx1x2Cyx1=Ex2x^2−Ex2Ex^2=E[x2(Ex2+Dx1Cov(x2,x1)(x1−Ex1))]−(Ex2)2=Dx1Cov(x2,x1)(Ex2x1−Ex2Ex1)=Dx1Cx1x22=D(Ex2+Dx1Cov(x2,x1)(x1−Ex1))=(Dx1)2Cx1x22Dx1=Dx1Cx1x22=Dx2+Dx^2−2Cov(x2,x^2)=Dx2−Dx1Cx1x22=Ey+Dx1Cx1y(x1−Ex1)+Dx2Cov(y,x2)(x2−Ex2)=OMIT+Dx2Cov(y,x2)−Cov(y,x^2)(x2−x^2)=OMIT+Dx2−Dx1Cx1x22Cyx2−Dx1Cx1x2Cyx1(x2−E2−Dx1Cov(x2,x1)(x1−E1))=OMIT+Dx1Dx2−Cx1x22Cyx2Dx1−Cx1x2Cyx1(x2−E2−Dx1Cx1x2(x1−E1))=Ey+A(x1−Ex1)+B(x2−E2)
其中OMIT\text{OMIT}OMIT用于代替式
Ey+Cx1yDx1(x1−Ex1)\text{E}y+\frac{C_{x_1y}}{\text{D}x_1}(x_1-\text{E}x_1) Ey+Dx1Cx1y(x1−Ex1)
以及
B=Cyx2Dx1−Cx1x2Cyx1Dx1Dx2−Cx1x22A=Cx1yDx1−BCx1x2Dx1=Cx1yDx1(Dx1Dx2−Cx1x22)−Cx1x2Dx1(Cyx2Dx1−Cx1x2Cyx1)Dx1Dx2−Cx1x22=Cx1yDx2−Cx1x2Cyx2Dx1Dx2−Cx1x22\begin{aligned} B &= \frac{C_{yx_2}\text{D}x_1-C_{x_1x_2}C_{yx_1}} {\text{D}x_1\text{D}x_2-C_{x_1x_2}^2} \\ A &= \frac{C_{x_1y}}{\text{D}x_1}-B\frac{C_{x_1x_2}}{\text{D}x_1} \\ &= \frac{\displaystyle\frac{C_{x_1y}}{\text{D}x_1} (\text{D}x_1\text{D}x_2-C_{x_1x_2}^2) -\displaystyle\frac{C_{x_1x_2}}{\text{D}x_1} (C_{yx_2}\text{D}x_1-C_{x_1x_2}C_{yx_1})} {\text{D}x_1\text{D}x_2-C_{x_1x_2}^2} \\ &= \frac{C_{x_1y}\text{D}x_2-C_{x_1x_2}C_{yx_2}} {\text{D}x_1\text{D}x_2-C_{x_1x_2}^2} \\ \end{aligned} BA=Dx1Dx2−Cx1x22Cyx2Dx1−Cx1x2Cyx1=Dx1Cx1y−BDx1Cx1x2=Dx1Dx2−Cx1x22Dx1Cx1y(Dx1Dx2−Cx1x22)−Dx1Cx1x2(Cyx2Dx1−Cx1x2Cyx1)=Dx1Dx2−Cx1x22Cx1yDx2−Cx1x2Cyx2
与另一个式子
E(y∣x1,x2)=Ey+[Cyx1Cyx2][Dx1Cx1x2Cx2x1Dx2]−1[x1−Ex1x2−Ex2]=Ey+[Cyx1Cyx2]Dx1Dx2−Cx1x22[Dx2−Cx1x2−Cx1x2Dx1][x1−Ex1x2−Ex2]\begin{aligned} \text{E}(y|x_1,x_2) &= \text{E}y+\left[ \begin{matrix} C_{yx_1} & C_{yx_2} \end{matrix}\right] \left[\begin{matrix} \text{D}x_1 & C_{x_1x_2} \\ C_{x_2x_1} & \text{D}x_2 \end{matrix}\right]^{-1} \left[\begin{matrix} x_1-\text{E}x_1 \\ x_2-\text{E}x_2 \end{matrix}\right] \\ &= \text{E}y+\frac{\left[ \begin{matrix} C_{yx_1} & C_{yx_2} \end{matrix}\right]} {\text{D}x_1\text{D}x_2-C^2_{x_1x_2}} \left[\begin{matrix} \text{D}x_2 & -C_{x_1x_2} \\ -C_{x_1x_2} & \text{D}x_1 \end{matrix}\right] \left[\begin{matrix} x_1-\text{E}x_1 \\ x_2-\text{E}x_2 \end{matrix}\right] \\ \end{aligned} E(y∣x1,x2)=Ey+[Cyx1Cyx2][Dx1Cx2x1Cx1x2Dx2]−1[x1−Ex1x2−Ex2]=Ey+Dx1Dx2−Cx1x22[Cyx1Cyx2][Dx2−Cx1x2−Cx1x2Dx1][x1−Ex1x2−Ex2]
中对应的AAA和BBB相等。

高斯白噪声中的直流电平

这个例子可以作为铺垫，有助于理解卡尔曼滤波各个公式的来源，比如x(k)x(k)x(k)和x(k−1)x(k-1)x(k−1)之间为什么还要有一个x^(k∣k−1)\hat{x}(k|k-1)x^(k∣k−1)等。考虑模型
x(k)=A+w(k)x(k)=A+w(k)x(k)=A+w(k)
其中AAA是待估计参数，w(k)w(k)w(k)是均值为0、方差为σ2\sigma^2σ2的高斯白噪声，x(k)x(k)x(k)是观测。这里需要注意的是，AAA也是一个先验随机变量，也就是说在测量AAA之前，预先猜测比方说AAA应该在10左右，大概率不超过7~13的范围，因此假设A∼N(10,1)A\sim N(10,1)A∼N(10,1)，然后开始测量。
一开始可以得到x^(0)=x(0)\hat{x}(0)=x(0)x^(0)=x(0)，然后根据x(0)x(0)x(0)和x(1)x(1)x(1)预测k=1k=1k=1时刻的值E[x(1)∣x(1),x(0)]\text{E}[x(1)|x(1),x(0)]E[x(1)∣x(1),x(0)]时，需要用到联合正态分布的条件可加性，但由于x(1)x(1)x(1)和x(0)x(0)x(0)不独立，需要使用投影定理计算出两个独立的变量x(0)x(0)x(0)与x~(1∣0)\widetilde{x}(1|0)x(1∣0)，进而计算x^(1)\hat{x}(1)x^(1)，即
x^(1)=E[x(1)∣x(1),x(0)]=E[x(1)∣x(0),x~(1∣0)]=E[x(1)∣x(0)]+E[x(1)∣x~(1∣0)]−Ex(1)\begin{aligned} \hat{x}(1) &= \text{E}[x(1)|x(1),x(0)] \\ &= \text{E}[x(1)|x(0),\widetilde{x}(1|0)] \\ &= \text{E}[x(1)|x(0)]+\text{E}[x(1)|\widetilde{x}(1|0)]-\text{E}x(1) \end{aligned} x^(1)=E[x(1)∣x(1),x(0)]=E[x(1)∣x(0),x(1∣0)]=E[x(1)∣x(0)]+E[x(1)∣x(1∣0)]−Ex(1)
其中E[x(1)∣x(0)]=x^(1∣0)\text{E}[x(1)|x(0)]=\hat{x}(1|0)E[x(1)∣x(0)]=x^(1∣0)，
x^(1∣0)=Ex(1)+Cov(x(1),x(0))Dx(0)(x(0)−Ex(0))=A+E(A+w(1))(A+w(0))−E(A+w(1))E(A+w(0))E(A+w(1))2−[E(A+w(1))]2(x(0)−A)=A\begin{aligned} \hat{x}(1|0) &= \text{E}x(1)+\frac{\text{Cov}(x(1),x(0))} {\text{D}x(0)}(x(0)-\text{E}x(0)) \\ &= A+\frac{\text{E}(A+w(1))(A+w(0))-\text{E}(A+w(1))\text{E}(A+w(0))} {\text{E}(A+w(1))^2-[\text{E}(A+w(1))]^2}(x(0)-A) \\ &= A \end{aligned} x^(1∣0)=Ex(1)+Dx(0)Cov(x(1),x(0))(x(0)−Ex(0))=A+E(A+w(1))2−[E(A+w(1))]2E(A+w(1))(A+w(0))−E(A+w(1))E(A+w(0))(x(0)−A)=A
此时式中就出现了x^(k∣k−1)\hat{x}(k|k-1)x^(k∣k−1)，和另一个未知式E[x(1)∣x~(1∣0)]\text{E}[x(1)|\widetilde{x}(1|0)]E[x(1)∣x(1∣0)]。由零均值应用定理，
E[x(1)∣x~(1∣0)]=Ex(0)+Cov(x(1),x~(1∣0))Dx~(1∣0)(x~(1∣0)−Ex~(1∣0))\text{E}[x(1)|\widetilde{x}(1|0)] = \text{E}x(0)+\frac{\text{Cov}(x(1),\widetilde{x}(1|0))} {\text{D}\widetilde{x}(1|0)}(\widetilde{x}(1|0)-\text{E}\widetilde{x}(1|0)) E[x(1)∣x(1∣0)]=Ex(0)+Dx(1∣0)Cov(x(1),x(1∣0))(x(1∣0)−Ex(1∣0))
其中
x~(1∣0)=x(1)−x^(1∣0)Ex~(1∣0)=E(x−x^)=0Dx~(1∣0)=E(x(1)−x^(1∣0))2=E(A+w(1))2=a2P(0)+σ2=P(1∣0)\begin{aligned} \widetilde{x}(1|0) &= x(1)-\hat{x}(1|0) \\ \text{E}\widetilde{x}(1|0) &= \text{E}(x-\hat{x}) = 0 \\ \text{D}\widetilde{x}(1|0) &= \text{E}(x(1)-\hat{x}(1|0))^2 \\ &= \text{E}(A+w(1))^2 \\ &= a^2P(0)+\sigma^2 \\ &= P(1|0) \end{aligned} x(1∣0)Ex(1∣0)Dx(1∣0)=x(1)−x^(1∣0)=E(x−x^)=0=E(x(1)−x^(1∣0))2=E(A+w(1))2=a2P(0)+σ2=P(1∣0)

卡尔曼滤波正式推导

标量形式
x^(k∣k−1)=E[x(k)∣Y(k−1)]=E[Ax(k−1)+Bu(k−1)+v(k−1)∣Y(k−1)]=AE[x(k−1)∣Y(k−1)]+BE[u(k−1)∣Y(k−1)]+E[v(k−1)∣Y(k−1)]=Ax^(k−1)+Bu(k−1)x~(k∣k−1)=x(k)−x^(k∣k−1)=[Ax(k−1)+Bu(k−1)+v(k−1)]−[Ax^(k−1)+Bu(k−1)]=Ax~(k−1)+v(k−1)y^(k∣k−1)=E[y(k)∣Y(k−1)]=E[Cx(k)+w(k−1)∣Y(k−1)]=Cx^(k∣k−1)\begin{aligned} \hat{x}(k|k-1) &= \text{E}[x(k)|Y(k-1)] \\ &= \text{E}[Ax(k-1)+Bu(k-1)+v(k-1)|Y(k-1)] \\ &= A\text{E}[x(k-1)|Y(k-1)]+B\text{E}[u(k-1)|Y(k-1)]+\text{E}[v(k-1)|Y(k-1)] \\ &= A\hat{x}(k-1)+Bu(k-1) \\ \widetilde{x}(k|k-1) &= x(k)-\hat{x}(k|k-1) \\ &= [Ax(k-1)+Bu(k-1)+v(k-1)]-[A\hat{x}(k-1)+Bu(k-1)] \\ &= A\widetilde{x}(k-1)+v(k-1) \\ \hat{y}(k|k-1) &= \text{E}[y(k)|Y(k-1)] \\ &= \text{E}[Cx(k)+w(k-1)|Y(k-1)] \\ &= C\hat{x}(k|k-1) \\ \end{aligned} x^(k∣k−1)x(k∣k−1)y^(k∣k−1)=E[x(k)∣Y(k−1)]=E[Ax(k−1)+Bu(k−1)+v(k−1)∣Y(k−1)]=AE[x(k−1)∣Y(k−1)]+BE[u(k−1)∣Y(k−1)]+E[v(k−1)∣Y(k−1)]=Ax^(k−1)+Bu(k−1)=x(k)−x^(k∣k−1)=[Ax(k−1)+Bu(k−1)+v(k−1)]−[Ax^(k−1)+Bu(k−1)]=Ax(k−1)+v(k−1)=E[y(k)∣Y(k−1)]=E[Cx(k)+w(k−1)∣Y(k−1)]=Cx^(k∣k−1)
由条件可加性，
x^(k)=E[x(k)∣Y(k)]=E[x(k)∣Y(k−1),y(k)]=E[x(k)∣Y(k−1),y~(k∣k−1)]=E[x(k)∣Y(k−1)]+E[x(k)∣y~(k∣k−1)]−Ex(k)=x^(k∣k−1)+E[x(k)∣y~(k∣k−1)]−Ex(k)\begin{aligned} \hat{x}(k) &= \text{E}[x(k)|Y(k)] \\ &= \text{E}[x(k)|Y(k-1),y(k)] \\ &= \text{E}[x(k)|Y(k-1),\widetilde{y}(k|k-1)] \\ &= \text{E}[x(k)|Y(k-1)]+\text{E}[x(k)|\widetilde{y}(k|k-1)]-\text{E}x(k) \\ &= \hat{x}(k|k-1)+\text{E}[x(k)|\widetilde{y}(k|k-1)]-\text{E}x(k) \end{aligned} x^(k)=E[x(k)∣Y(k)]=E[x(k)∣Y(k−1),y(k)]=E[x(k)∣Y(k−1),y(k∣k−1)]=E[x(k)∣Y(k−1)]+E[x(k)∣y(k∣k−1)]−Ex(k)=x^(k∣k−1)+E[x(k)∣y(k∣k−1)]−Ex(k)
由零均值应用定理，
E[x(k)∣y~(k∣k−1)]=Ex(k)+Cov(x(k),y~(k∣k−1))Dy~(k∣k−1)(y~(k∣k−1)−Ey~(k∣k−1))\text{E}[x(k)|\widetilde{y}(k|k-1)] = \text{E}x(k)+\frac{\text{Cov}(x(k),\widetilde{y}(k|k-1))} {\text{D}\widetilde{y}(k|k-1)}(\widetilde{y}(k|k-1)-\text{E}\widetilde{y}(k|k-1)) E[x(k)∣y(k∣k−1)]=Ex(k)+Dy(k∣k−1)Cov(x(k),y(k∣k−1))(y(k∣k−1)−Ey(k∣k−1))
其中
Ey~(k∣k−1)=E(y−y^)=0P(k∣k−1)=Dx~(k∣k−1)=E(ax~(k−1)+v(k−1))2=a2P(k−1)+V\begin{aligned} \text{E}\widetilde{y}(k|k-1) &= \text{E}(y-\hat{y}) = 0 \\ P(k|k-1) &= \text{D}\widetilde{x}(k|k-1) \\ &= \text{E}(a\widetilde{x}(k-1)+v(k-1))^2 \\ &= a^2P(k-1)+V \end{aligned} Ey(k∣k−1)P(k∣k−1)=E(y−y^)=0=Dx(k∣k−1)=E(ax(k−1)+v(k−1))2=a2P(k−1)+V
令
K(k)=Cov(x(k),y~(k∣k−1))Dy~(k∣k−1)K(k)=\frac{\text{Cov}(x(k),\widetilde{y}(k|k-1))} {\text{D}\widetilde{y}(k|k-1)} K(k)=Dy(k∣k−1)Cov(x(k),y(k∣k−1))
则
E[x(k)∣y~(k∣k−1)]=Ex(k)+K(k)(y(k)−y^(k∣k−1))x^(k)=x^(k∣k−1)+E[x(k)∣y~(k∣k−1)]−Ex(k)=x^(k∣k−1)+K(k)(y(k)−Cx^(k∣k−1))\begin{aligned} \text{E}[x(k)|\widetilde{y}(k|k-1)] &= \text{E}x(k)+K(k)(y(k)-\hat{y}(k|k-1)) \\ \hat{x}(k)&= \hat{x}(k|k-1)+\text{E}[x(k)|\widetilde{y}(k|k-1)]-\text{E}x(k) \\ &= \hat{x}(k|k-1)+K(k)(y(k)-C\hat{x}(k|k-1)) \end{aligned} E[x(k)∣y(k∣k−1)]x^(k)=Ex(k)+K(k)(y(k)−y^(k∣k−1))=x^(k∣k−1)+E[x(k)∣y(k∣k−1)]−Ex(k)=x^(k∣k−1)+K(k)(y(k)−Cx^(k∣k−1))
下面计算其中的K(k)K(k)K(k)，
Dy~(k∣k−1)=D[Cx(k)+w(k)−Cx^(k∣k−1)]=C2D[x(k)−x^(k∣k−1)]+Dw(k)=C2P(k∣k−1)+WCov(x(k),y~(k∣k−1))=Cov(x(k),Cx(k)+w(k)−Cx^(k∣k−1))=CCov(x(k),x(k)−x^(k∣k−1))=CE[x(k)(x(k)−x^(k∣k−1))]−CEx(k)E[(x(k)−x^(k∣k−1))]=CE[(x(k)((x(k)−x^(k∣k−1))]=CE[(x(k)((x(k)−x^(k∣k−1))−x^(k∣k−1)((x(k)−x^(k∣k−1))]=CE[(x(k)−x^(k∣k−1))2]=CP(k∣k−1)K(k)=Cov(x(k),y~(k∣k−1))Dy~(k∣k−1)=CP(k∣k−1)C2P(k∣k−1)+W\begin{aligned} \text{D}\widetilde{y}(k|k-1) &= \text{D}[Cx(k)+w(k)-C\hat{x}(k|k-1)] \\ &= C^2\text{D}[x(k)-\hat{x}(k|k-1)]+\text{D}w(k) \\ &= C^2P(k|k-1)+W \\ \text{Cov}(x(k),\widetilde{y}(k|k-1)) &= \text{Cov}(x(k),Cx(k)+w(k)-C\hat{x}(k|k-1)) \\ &= C\text{Cov}(x(k),x(k)-\hat{x}(k|k-1)) \\ &= C\text{E}[x(k)(x(k)-\hat{x}(k|k-1))] -C\text{E}x(k)\text{E}[(x(k)-\hat{x}(k|k-1))] \\ &= C\text{E}[(x(k)((x(k)-\hat{x}(k|k-1))] \\ &= C\text{E}[(x(k)((x(k)-\hat{x}(k|k-1)) -\hat{x}(k|k-1)((x(k)-\hat{x}(k|k-1))] \\ &= C\text{E}[(x(k)-\hat{x}(k|k-1))^2] \\ &= CP(k|k-1) \\ K(k) &= \frac{\text{Cov}(x(k),\widetilde{y}(k|k-1))} {\text{D}\widetilde{y}(k|k-1)} \\ &= \frac{CP(k|k-1)}{C^2P(k|k-1)+W} \\ \end{aligned} Dy(k∣k−1)Cov(x(k),y(k∣k−1))K(k)=D[Cx(k)+w(k)−Cx^(k∣k−1)]=C2D[x(k)−x^(k∣k−1)]+Dw(k)=C2P(k∣k−1)+W=Cov(x(k),Cx(k)+w(k)−Cx^(k∣k−1))=CCov(x(k),x(k)−x^(k∣k−1))=CE[x(k)(x(k)−x^(k∣k−1))]−CEx(k)E[(x(k)−x^(k∣k−1))]=CE[(x(k)((x(k)−x^(k∣k−1))]=CE[(x(k)((x(k)−x^(k∣k−1))−x^(k∣k−1)((x(k)−x^(k∣k−1))]=CE[(x(k)−x^(k∣k−1))2]=CP(k∣k−1)=Dy(k∣k−1)Cov(x(k),y(k∣k−1))=C2P(k∣k−1)+WCP(k∣k−1)
其中显然CEx(k)E[(x(k)−x^(k∣k−1))]=0C\text{E}x(k)\text{E}[(x(k)-\hat{x}(k|k-1))]=0CEx(k)E[(x(k)−x^(k∣k−1))]=0，而x^(k∣k−1)((x(k)−x^(k∣k−1))]\hat{x}(k|k-1)((x(k)-\hat{x}(k|k-1))]x^(k∣k−1)((x(k)−x^(k∣k−1))]是由投影定理的下面两个公式中的第二个
E[y(x−x^)]=0,E[x^(x−x^)]=0\text{E}[y(x-\hat{x})]=0,\quad\text{E}[\hat{x}(x-\hat{x})]=0 E[y(x−x^)]=0,E[x^(x−x^)]=0
下面出于递推需要而计算P(k)P(k)P(k)
P(k)=D[x(k)−x^(k)]=D[x(k)−x^(k∣k−1)−K(k)(y(k)−Cx^(k∣k−1))]=D[x(k)−x^(k∣k−1)−K(k)C(x(k)−x^(k∣k−1))−K(k)w]=D[(1−K(k)C)x~−K(k)w]=(1−K(k)C)2P(k∣k−1)+K2(k)W=(WC2P(k∣k−1)+W)2P(k∣k−1)+(CP(k∣k−1)C2P(k∣k−1)+W)2W=W2P(k∣k−1)+C2P2(k∣k−1)W(C2P(k∣k−1)+W)2=WP(k∣k−1)C2P(k∣k−1)+W=(1−K(k)C)P(k∣k−1)\begin{aligned} P(k) &= \text{D}[x(k)-\hat{x}(k)] \\ &= \text{D}[x(k)-\hat{x}(k|k-1)-K(k)(y(k)-C\hat{x}(k|k-1))] \\ &= \text{D}[x(k)-\hat{x}(k|k-1)-K(k)C(x(k)-\hat{x}(k|k-1))-K(k)w] \\ &= \text{D}[(1-K(k)C)\widetilde{x}-K(k)w] \\ &= (1-K(k)C)^2P(k|k-1)+K^2(k)W \\ &= \left(\frac{W}{C^2P(k|k-1)+W}\right)^2P(k|k-1) +\left(\frac{CP(k|k-1)}{C^2P(k|k-1)+W}\right)^2W \\ &= \frac{W^2P(k|k-1)+C^2P^2(k|k-1)W}{(C^2P(k|k-1)+W)^2} \\ &= \frac{WP(k|k-1)}{C^2P(k|k-1)+W} \\ &= (1-K(k)C)P(k|k-1) \\ \end{aligned} P(k)=D[x(k)−x^(k)]=D[x(k)−x^(k∣k−1)−K(k)(y(k)−Cx^(k∣k−1))]=D[x(k)−x^(k∣k−1)−K(k)C(x(k)−x^(k∣k−1))−K(k)w]=D[(1−K(k)C)x−K(k)w]=(1−K(k)C)2P(k∣k−1)+K2(k)W=(C2P(k∣k−1)+WW)2P(k∣k−1)+(C2P(k∣k−1)+WCP(k∣k−1))2W=(C2P(k∣k−1)+W)2W2P(k∣k−1)+C2P2(k∣k−1)W=C2P(k∣k−1)+WWP(k∣k−1)=(1−K(k)C)P(k∣k−1)
向量形式
x^(k∣k−1)=E[x(k)∣Y(k−1)]=E[Ax(k−1)+Bu(k−1)+v(k−1)∣Y(k−1)]=AE[x(k−1)∣Y(k−1)]+BE[u(k−1)∣Y(k−1)]+E[v(k−1)∣Y(k−1)]=Ax^(k−1)+Bu(k−1)\begin{aligned} \hat{x}(k|k-1) &= \text{E}[x(k)|Y(k-1)] \\ &= \text{E}[Ax(k-1)+Bu(k-1)+v(k-1)|Y(k-1)] \\ &= A\text{E}[x(k-1)|Y(k-1)]+B\text{E}[u(k-1)|Y(k-1)]+\text{E}[v(k-1)|Y(k-1)] \\ &= A\hat{x}(k-1)+Bu(k-1) \\ \end{aligned} x^(k∣k−1)=E[x(k)∣Y(k−1)]=E[Ax(k−1)+Bu(k−1)+v(k−1)∣Y(k−1)]=AE[x(k−1)∣Y(k−1)]+BE[u(k−1)∣Y(k−1)]+E[v(k−1)∣Y(k−1)]=Ax^(k−1)+Bu(k−1)

参考

孙增圻. 计算机控制理论与应用[M]. 清华大学出版社, 2008.
StevenM.Kay, 罗鹏飞. 统计信号处理基础[M]. 电子工业出版社, 2014.
赵树杰, 赵建勋. 信号检测与估计理论[M]. 电子工业出版社, 2013.
卡尔曼滤波的推导过程详解

自用的卡尔曼滤波推导相关推荐

卡尔曼滤波推导思路总结
推导思路一: (1) 混合高斯一维高斯函数形式: (1)N(x,μ,σ)=1σ2πe−(x−μ)22σ2\mathcal N(x,\mu,\sigma)=\frac{1}{\sigma\sqrt{2 ...
主流卡尔曼滤波推导——KF、EKF、IKF、UKF、ESKF
文章目录一.高斯分布 1.1 高斯概率密度函数 1.2 联合高斯概率密度函数 1.3 高斯随机变量的线性变换二.滤波器基本原理 2.1 贝叶斯滤波三.卡尔曼滤波 3.1 普通卡尔曼滤波器 (KF ...
概率机器人总结——(扩展)卡尔曼滤波先实践再推导
概率机器人总结--卡尔曼滤波先实践再推导概率机器人总结--(扩展)卡尔曼滤波先实践再推导 (1)卡尔曼.扩展卡尔曼.粒子滤波到底什么关系? (2)实践--扩展卡尔曼滤波 (3)推导--卡尔曼滤波 ( ...
卡尔曼滤波的推导过程详解
在学习卡尔曼滤波的时候看了很多博客讲这方面的知识,感觉都讲得表面的东西,无法了解它五个公式真正代表的过程,这篇博客我想以我的理解讲讲卡尔曼滤波. 首先我先写出卡尔曼滤波的具体过程,首先针对如下状态空间 ...
卡尔曼滤波(kalman filter)超详细推导
1. 概率论相关知识这一节主要回忆概率论的一些相关基础知识,包括全概率公式.贝叶斯公式.协方差矩阵.多维高斯分布等等,对这些熟悉的可以直接跳到第2节看贝叶斯滤波 1.1 条件概率 P(x,y)=P( ...
实践卡尔曼滤波--小球追踪
目标追踪之卡尔曼滤波最近在看Coursera的robotic learning,发现挺有意思的.这里算是做一下week 2的用卡尔曼滤波来做机器人目标追踪的笔记. 这篇小文章主要有两个内容怎样用一 ...
【多源融合】自适应卡尔曼滤波的多种形式：遗忘卡尔曼滤波、渐消记忆卡尔曼滤波和自适应卡尔曼滤波
自适应卡尔曼滤波的多种形式在卡尔曼滤波量测更新过程中,按照平差中的数学模型分为函数模型和随机模型,那么动力学方法和观测方程均为函数模型,P和R矩阵的确定则为随机模型的建立. 对于P矩阵来说,其核心思 ...
扩展卡尔曼滤波建模及应用
一.公式 1.卡尔曼滤波 A:状态向量 F:状态转移矩阵 P:状态协方差矩阵 Q:过程噪声矩阵 Z:测量向量 H:测量矩阵 R:测量噪声矩阵本质上讲,建立卡尔曼滤波数学模型的 ...
【EKF】卡尔曼滤波原理
前言本篇是最近在学习云台以及姿态融合时,想学习一下卡尔曼滤波的原理,基于 DR_CAN 老师的卡尔曼滤波的视频做的笔记,基本上是按照 DR_CAN 老师的思路自己推导计算了一遍,推荐还是看一下原视频 ...

自用的卡尔曼滤波推导