参考：古月居-卡尔曼滤波

一、高斯分布（Gaussian Distribution）

• 一维高斯分布：随机变量 X X X服从数学期望 μ \mu μ、方差 σ 2 \sigma^2 σ2的正态分布，概率密度函数为 p ( x ) p(x) p(x)。

  X ∼ N ( μ , σ 2 ) X \sim N(\mu, \sigma^2) X∼N(μ,σ2)

  p ( x ) = 1 2 π σ e x p ( − 1 2 ( x − μ ) 2 σ 2 ) p(x) = \frac{1}{\sqrt{2\pi}\sigma}exp(-\frac{1}{2}\frac{(x-\mu)^2}{\sigma^2}) p(x)=2π ​σ1​exp(−21​σ2(x−μ)2​)

• 高维高斯分布：数学期望 μ \mu μ为 1 × n 1×n 1×n的矩阵， Σ \Sigma Σ为 n × n n×n n×n的协方差矩阵。

  X ∼ N ( μ , Σ ) X \sim N(\mu, \Sigma) X∼N(μ,Σ)

  p ( x ) = 1 ( 2 π ) n d e t ( Σ ) e x p ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) p(x) = \frac{1}{\sqrt{(2\pi)^ndet(\Sigma)}}exp(-\frac{1}{2}(x - \mu)^T\Sigma^{-1}(x-\mu)) p(x)=(2π)ndet(Σ) ​1​exp(−21​(x−μ)TΣ−1(x−μ))

  注：

  1. 总体方差 σ 2 \sigma^2 σ2

     x i x_i xi​是表示第 i i i个样本的观测值， N N N是总体样本数量， n n n是采样样本数量， μ \mu μ是总体均值（ μ = 1 N Σ i = 1 N x i \mu=\frac{1}{N}{\Sigma}_{i=1}^{N}x_i μ=N1​Σi=1N​xi​）， x ˉ \bar{x} xˉ是采样样本均值（ x ˉ = 1 n Σ i = 1 n x i \bar{x}=\frac{1}{n}{\Sigma}_{i=1}^{n}x_i xˉ=n1​Σi=1n​xi​）。根据n个采样样本的方差估计总体方差，有如下两种情况。

     （1）如果总体均值 μ \mu μ已知，那么总体方差：

     σ 2 = Σ i = 1 N ( x i − μ ) 2 N (1) \sigma^2=\frac{{\Sigma}_{i=1}^N(x_i-\mu)^2}{N} \tag{1} σ2=NΣi=1N​(xi​−μ)2​(1)

     （2）在总体均值未知的情况下，根据样本方差估计总体方差，则分为有偏估计和无偏估计。

  • 有偏估计：

    σ 2 = Σ i = 1 n ( x i − x ˉ ) 2 n (2) \sigma^2=\frac{{\Sigma}_{i=1}^n(x_i-\bar{x})^2}{n} \tag{2} σ2=nΣi=1n​(xi​−xˉ)2​(2)

  • 无偏估计

    σ 2 = Σ i = 1 n ( x i − x ˉ ) 2 n − 1 (3) \sigma^2=\frac{{\Sigma}_{i=1}^n(x_i-\bar{x})^2}{n-1} \tag{3} σ2=n−1Σi=1n​(xi​−xˉ)2​(3)

    2. 协方差

       在无偏估计的基础上，协方差的定义如下，其中 x ˉ \bar{x} xˉ和 y ˉ \bar{y} yˉ​分别表示两个随机变量对应的观测样本均值。

       σ ( x , y ) = 1 n − 1 Σ i = 1 n ( x i − x ˉ ) ( y i − y ˉ ) ) (4) \sigma(x,y)=\frac{1}{n-1}\Sigma_{i=1}^{n}(x_i-\bar{x})(y_i-\bar{y})) \tag{4} σ(x,y)=n−11​Σi=1n​(xi​−xˉ)(yi​−yˉ​))(4)

       则公式(3)可以写成如下格式，表示随机变量 x x x关于其自身的协方差。

       σ 2 = Σ i = 1 n ( x i − x ˉ ) 2 n − 1 = σ ( x , x ) \sigma^2=\frac{{\Sigma}_{i=1}^n(x_i-\bar{x})^2}{n-1} =\sigma(x,x) σ2=n−1Σi=1n​(xi​−xˉ)2​=σ(x,x)

    3. 协方差矩阵

       给定d个随机变量 X = { x 1 , x 2 , x 3 , . . . , x d } X=\{x_1, x_2, x_3, ..., x_d\} X={x1​,x2​,x3​,...,xd​}，有 x k ∈ X , k = 1 , 2 , . . . , d x_k\in{X},k=1,2,...,d xk​∈X,k=1,2,...,d，则这些随机变量的方差为：

       σ ( x k , x k ) = 1 n − 1 Σ i = 1 n ( x k i − x ˉ k ) \sigma(x_k, x_k)=\frac{1}{n-1}\Sigma_{i=1}^n(x_{ki}-{\bar{x}}_k) σ(xk​,xk​)=n−11​Σi=1n​(xki​−xˉk​)

       其中， x k i x_{ki} xki​表示第 k k k个随机变量 x k x_{k} xk​的第 i i i个观测值， n n n表示采样样本数量，每个随机变量均采样了 n n n个样本。对于这些随机变量，求两两之间的协方差，则有：

       σ ( x m , x k ) = 1 n − 1 Σ i = 1 n ( x m i − x ˉ m ) ( x k i − x ˉ k ) \sigma(x_m, x_k)=\frac{1}{n-1}\Sigma_{i=1}^n(x_{mi}-{\bar{x}}_m)(x_{ki}-{\bar{x}}_k) σ(xm​,xk​)=n−11​Σi=1n​(xmi​−xˉm​)(xki​−xˉk​)

       所以，协方差矩阵为：

       Σ = [ σ ( x 1 , x 1 ) . . . σ ( x 1 , x d ) ⋮ ⋱ ⋮ σ ( x d , x 1 ) . . . σ ( x d , x d ) ] ∈ R d × d \Sigma = \begin{bmatrix} \sigma(x_1, x_1) & ... & \sigma(x_1, x_d)\\ \vdots & \ddots & \vdots \\ \sigma(x_d, x_1) & ... & \sigma(x_d, x_d)\end{bmatrix} \in{\R^{d×d}} Σ= ​σ(x1​,x1​)⋮σ(xd​,x1​)​...⋱...​σ(x1​,xd​)⋮σ(xd​,xd​)​ ​∈Rd×d

    参考：总体方差的有偏与无偏估计、高斯分布、如何理解多维高斯分布？

  二、卡尔曼滤波

  经典线性卡尔曼滤波推导

  【参考：一文看懂卡尔曼滤波、卡尔曼滤波算法详细推导】

  • 建模

    已知一辆汽车直线行驶，已知它在 t − 1 t-1 t−1时刻的位置是 p t − 1 p_{t-1} pt−1​，速度是 v t − 1 v_{t-1} vt−1​，加速度是 u u u， t t t时刻与 t − 1 t-1 t−1时刻的时间差为 Δ t \Delta_{t} Δt​。理想情况下，根据速度、位移、加速度之间的关系，它在 t t t时刻的位置 p t p_{t} pt​和速度 v t v_{t} vt​ 可以表述为：

    v t = v t − 1 + u Δ t (1) v_t=v_{t-1}+u\Delta_{t} \tag{1} vt​=vt−1​+uΔt​(1)

    p t = p t − 1 + u t − 1 Δ t + 1 2 u ( Δ t ) 2 (2) p_t=p_{t-1}+u_{t-1}\Delta_{t}+\frac{1}{2} u{(\Delta_{t})}^2\tag{2} pt​=pt−1​+ut−1​Δt​+21​u(Δt​)2(2)

    直接根据上一时刻状态预测下一时刻的状态得到的预测结果没有考虑过程误差和传感器测量误差。实际情况误差是存在的，卡尔曼滤波即是通过观测信息寻求一个最优的状态估计。上式写成矩阵形式：

    [ p t v t ] = [ 1 Δ t 0 1 ] [ p t − 1 v t − 1 ] + [ ( Δ t ) 2 2 Δ t ] u (3) \begin{bmatrix} p_t \\ v_t \end{bmatrix}=\begin{bmatrix} 1 & {\Delta}_t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} p_{t -1}\\ v_{t -1} \end{bmatrix} + \begin{bmatrix} \frac{({\Delta}_t)^2}{2} \\ {\Delta}_t \end{bmatrix}u \tag{3} [pt​vt​​]=[10​Δt​1​][pt−1​vt−1​​]+[2(Δt​)2​Δt​​]u(3)

    即可写成：

    x t = F x t − 1 + B u (4) x_t=Fx_{t-1}+Bu \tag{4} xt​=Fxt−1​+Bu(4)

    考虑过程误差 w t w_t wt​，上式可写成：

    x t = F x t − 1 + B u + w t (5) x_t=Fx_{t-1}+Bu+w_{t} \tag{5} xt​=Fxt−1​+Bu+wt​(5)

    通过传感器观测可对 t t t时刻的速度和位置进行测量，假设直接预测结果与传感器观测的结果之间是线性变换关系，并考虑传感器测量误差 v t v_t vt​，则观测方程可写为：

    z t = H x t + v t (6) z_t=Hx_t+v_t \tag{6} zt​=Hxt​+vt​(6)

    一般假设过程误差和观测误差服从高斯分布，即 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 Q Q和 R R R为协方差矩阵，即 Q = E [ W W T ] Q=E[WW^T] Q=E[WWT]和 R = E [ V V T ] R=E[VV^T] R=E[VVT]。 Q Q Q和 R R R为超参数，根据经验设定，推导过程中认为已知。

    • 变量定义

      x t x_t xt​：状态真实值

      x ~ t − {\tilde{x}_t^-} x~t−​：直接根据上一时刻的状态预测的结果（没有考虑观测信息，先验）

      x ~ t \tilde{x}_t x~t​：根据观测结果进行优化得到的最优估计值（考虑观测信息，后验）

      先验误差： e t − = x t − x ~ t − {e_t^-}=x_t-{\tilde{x}_t^-} et−​=xt​−x~t−​

      后验误差： e t = x t − x ~ t e_t=x_t-\tilde{x}_t et​=xt​−x~t​

      先验误差协方差矩阵： P t − = E [ e t − ( e t − ) T ] {P_t^-}=E[e_t^-({{e}_t^-)^T}] Pt−​=E[et−​(et−​)T]

      后验误差协方差矩阵： P t = E [ e t e t T ] {P_t}=E[e_t{{e}_t^T}] Pt​=E[et​etT​]

    • 公式推导
      1. 真实状态空间

         状态真实值： x t = F x t − 1 + B u + w t x_t = Fx_{t-1}+Bu+w_t xt​=Fxt−1​+Bu+wt​，其中 p ( W ) ∼ N ( 0 , Q ) , Q = E [ W W T ] p(W)\sim{N(0,Q)}, Q=E[WW^T] p(W)∼N(0,Q),Q=E[WWT]

         状态观测值： z t = H x t + v t z_t=Hx_t+v_t zt​=Hxt​+vt​，其中 p ( V ) ∼ N ( 0 , R ) , R = E [ V V T ] p(V)\sim{N(0,R)}, R=E[VV^T] p(V)∼N(0,R),R=E[VVT]

      2. 实际状态空间

         直接预测值： x ~ t − = F x ~ t − 1 + B u \tilde{x}_t^-=F\tilde{x}_{t-1}+Bu x~t−​=Fx~t−1​+Bu

         校正后状态： x ~ t = x ~ t − + K t ( z t − H x ~ t − ) \tilde{x}_t=\tilde{x}_t^-+{K_t}(z_t-H\tilde{x}_t^-) x~t​=x~t−​+Kt​(zt​−Hx~t−​)，其中 K t ( z t − H x ~ t − ) {K_t}(z_t-H\tilde{x}_t^-) Kt​(zt​−Hx~t−​)表示对预测值的修正

      寻找 K t K_t Kt​使 x ~ t \tilde{x}_t x~t​尽可能接近 x t x_t xt​，即使后验误差 e t = x t − x ~ t e_t=x_t-\tilde{x}_t et​=xt​−x~t​最小化。

      。。。待敲公式

      卡尔曼增益：

      后验估计：

      更新误差协方差：

      扩展卡尔曼理解参考：卡尔曼滤波器之扩展卡尔曼滤波