以高斯分布的视角理解卡尔曼滤波原理

帅帅爱数学
116 阅读3分钟

什么是卡尔曼滤波

卡尔曼滤波的作用是在含有不确定信息的动态系统中，根据上一步状态的最优估计，对系统下一步的走向做出有根据的预测。

在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。

卡尔曼滤波原理

概念

状态变量 x\mathrm{x}： 表示系统的运动状态，如速度，位置等。

状态协方差矩阵 P\mathrm{P}： 表示各个状态变量之间的关系，将运动状态看做高斯分布，则状态变量与状态协方差矩阵分别作为高斯分布的均值与方差。

状态协方差噪声 Q\mathrm{Q}： 表示由于外部环境干扰的因素，导致下一时刻状态服从的高斯分布方差的变化。

状态转移矩阵 F\mathrm{F}： 根据运动方程，时状态变量从上一时刻到下一时刻的变换矩阵。

观测变量 z\mathrm{z}： 表示系统中当前时刻的传感器的测量值。

观测协方差矩阵 R\mathrm{R}： 表示各个观测值之间的关系，将观测值看做高斯分布，则观测值与观测协方差矩阵分别作为高斯分布的均值与方差。

状态到观测的转移矩阵 H\mathrm{H}： 表示从状态变量空间到观测空间的变换矩阵。

原理

以K−1K-1时刻的状态最优估计xK−1\mathrm{x}_{K-1}，预测KK时刻的状态x^K\hat{\mathrm{x}}_K，同时对KK时刻的状态进行观测得到观测值zK\mathrm{z}_K，利用zK\mathrm{z}_K对x^K\hat{\mathrm{x}}_K进行修正，最终得到KK时刻的状态最优估计xK\mathrm{x}_K. 卡尔曼滤波算法假设状态变量与观测变量都各自符合高斯分布，融合状态变量与观测变量即将两个高斯分布融合为一个高斯分布，得到新的均值和方差，新的均值作为状态最优估计。

1、预测过程，预测KK时刻的状态x^K\hat{\mathrm{x}}_K和协方差矩阵PK\mathrm{P}_K

预测KK时刻的状态x^K\hat{\mathrm{x}}_K

x^K=F⋅xK−1\hat{\mathrm{x}}_K = \mathrm{F}\cdot\mathrm{x}_{K-1}

相应地，K−1K-1时刻的状态协方差矩阵PK−1\mathrm{P}_{K-1}更新为KK时刻的状态预测协方差矩阵P^k\hat{\mathrm{P}}_k。根据协方差矩阵的计算

P^K=F⋅PK−1⋅FT+Q\hat{\mathrm{P}}_K=\mathrm{F}\cdot\mathrm{P}_{K-1}\cdot\mathrm{F}^T+\mathrm{Q}

2、状态更新过程，利用zK\mathrm{z}_K对x^K\hat{\mathrm{x}}_K进行修正

将KK时刻的状态预测x^K\hat{\mathrm{x}}_K和协方差P^K\hat{\mathrm{P}}_K，变换到观测变量空间

uK=H⋅x^KΣK=H⋅P^K⋅HT\begin{aligned} &\mathrm{u}_K=\mathrm{H}\cdot\hat{\mathrm{x}}_K \\ &\Sigma_K=\mathrm{H}\cdot\hat{\mathrm{P}}_K\cdot\mathrm{H}^T \end{aligned}

KK时刻的观测值和测观测协方差(zK,RK)(\mathrm{z}_K,\mathrm{R}_K)与空间变换之后的状态预测值和协方差(uK,ΣK)(\mathrm{u}_K,\Sigma_K)是同空间中的两个高斯分布，融合两个高斯分布得到新的高斯分布的变量和协方差（融合推导见文章底部**“附：两个高斯分布的融合”**）

u=uK+ΣK⋅(ΣK+RK)−1⋅(zK−uK)Σ=ΣK−ΣK⋅(ΣK+RK)−1⋅ΣK\begin{aligned} &\mathrm{u}=\mathrm{u}_K+\Sigma_K\cdot(\Sigma_K+\mathrm{R}_K)^{-1}\cdot(\mathrm{z}_K-\mathrm{u}_K)\\ &\Sigma=\Sigma_K-\Sigma_K\cdot(\Sigma_K+\mathrm{R}_K)^{-1}\cdot\Sigma_K \end{aligned}

其中u=H⋅xK\mathrm{u}=\mathrm{H}\cdot\mathrm{x}_K，Σ=H⋅PK⋅HT\Sigma=\mathrm{H}\cdot\mathrm{P}_K\cdot\mathrm{H}^T.

令K=ΣK⋅(ΣK+RK)−1\mathrm{K}=\Sigma_K\cdot(\Sigma_K+\mathrm{R}_K)^{-1}，则有

H⋅xK=H⋅x^K+K⋅(zK−H⋅x^K)H⋅PK⋅HT=H⋅P^K⋅HT−K⋅ΣK\begin{aligned} &\mathrm{H}\cdot\mathrm{x}_K=\mathrm{H}\cdot\hat{\mathrm{x}}_K+\mathrm{K}\cdot(\mathrm{z}_K-\mathrm{H}\cdot\hat{\mathrm{x}}_K)\\ &\mathrm{H}\cdot\mathrm{P}_K\cdot\mathrm{H}^T=\mathrm{H}\cdot\hat{\mathrm{P}}_K\cdot\mathrm{H}^T-\mathrm{K}\cdot\Sigma_K \end{aligned}

因此有

K=H⋅P^K⋅HT⋅(H⋅P^K⋅HT+RK)−1xK=x^K+H−1⋅K⋅(zK−H⋅x^K)PK=P^K−H−1⋅K⋅H⋅P^K\begin{aligned} &\mathrm{K}=\mathrm{H}\cdot\hat{\mathrm{P}}_K\cdot\mathrm{H}^T\cdot(\mathrm{H}\cdot\hat{\mathrm{P}}_K\cdot\mathrm{H}^T+\mathrm{R}_K)^{-1}\\ &\mathrm{x}_K=\hat{\mathrm{x}}_K+\mathrm{H}^{-1}\cdot\mathrm{K}\cdot(\mathrm{z}_K-\mathrm{H}\cdot\hat{\mathrm{x}}_K)\\ &\mathrm{P}_K=\hat{\mathrm{P}}_K-\mathrm{H}^{-1}\cdot\mathrm{K}\cdot\mathrm{H}\cdot\hat{\mathrm{P}}_K \end{aligned}

令K′=H−1⋅K=P^K⋅HT⋅(H⋅P^K⋅HT+RK)−1\mathrm{K}'=\mathrm{H}^{-1}\cdot\mathrm{K}=\hat{\mathrm{P}}_K\cdot\mathrm{H}^T\cdot(\mathrm{H}\cdot\hat{\mathrm{P}}_K\cdot\mathrm{H}^T+\mathrm{R}_K)^{-1}，有

K′=P^K⋅HT⋅(H⋅P^K⋅HT+RK)−1xK=x^K+K′⋅(zK−H⋅x^K)PK=P^K−K′⋅H⋅P^K\begin{aligned} &\mathrm{K}'=\hat{\mathrm{P}}_K\cdot\mathrm{H}^T\cdot(\mathrm{H}\cdot\hat{\mathrm{P}}_K\cdot\mathrm{H}^T+\mathrm{R}_K)^{-1}\\ &\mathrm{x}_K=\hat{\mathrm{x}}_K+\mathrm{K}'\cdot(\mathrm{z}_K-\mathrm{H}\cdot\hat{\mathrm{x}}_K)\\ &\mathrm{P}_K=\hat{\mathrm{P}}_K-\mathrm{K}'\cdot\mathrm{H}\cdot\hat{\mathrm{P}}_K \end{aligned}

附：两个高斯分布的融合

两个nn维高斯分布X∼(u1,Σ1)X\sim(\mathrm{u}_1,\Sigma_1)和Y∼(u2,Σ2)Y\sim(\mathrm{u}_2,\Sigma_2)，令融合后的高斯分布为Z∼(u,Σ)Z\sim(\mathrm{u},\Sigma)，其中Z=AX+BYZ=AX+BY，A+B=IA+B=I. 则

u=Au1+Bu2Σ=AΣ1AT+BΣ2BT\begin{aligned} &\mathrm{u}=A\mathrm{u}_1+B\mathrm{u}_2\\ &\Sigma=A\Sigma_1A^T+B\Sigma_2B^T \end{aligned}

融合后的高斯分布方差越小，则均值u\mathrm{u}越接近真实情况。

将A+B=IA+B=I带入Σ=AΣ1AT+BΣ2BT\Sigma=A\Sigma_1A^T+B\Sigma_2B^T，得

Σ=AΣ1AT+(I−A)Σ2(I−A)T=A(Σ1+Σ2)AT+Σ2−AΣ2−Σ2AT\Sigma=A\Sigma_1A^T+(I-A)\Sigma_2(I-A)^T=A(\Sigma_1+\Sigma_2)A^T+\Sigma_2-A\Sigma_2-\Sigma_2A^T

令dΣ=0\mathrm{d}\Sigma=0，得

dΣ=(dA)(Σ1+Σ2)AT+A(Σ1+Σ2)(dA)T−(dA)Σ2−Σ2(dA)T=(dA)((Σ1+Σ2)AT−Σ2)+(A(Σ1+Σ2)−Σ2)(dA)T\begin{aligned} \mathrm{d}\Sigma &= (\mathrm{d}A)(\Sigma_1+\Sigma_2)A^T+A(\Sigma_1+\Sigma_2)(\mathrm{d}A)^T-(\mathrm{d}A)\Sigma_2-\Sigma_2(\mathrm{d}A)^T \\ &= (\mathrm{d}A)((\Sigma_1+\Sigma_2)A^T-\Sigma_2)+(A(\Sigma_1+\Sigma_2)-\Sigma_2)(\mathrm{d}A)^T \end{aligned}

因此A=Σ2(Σ1+Σ2)−1A=\Sigma_2(\Sigma_1+\Sigma_2)^{-1}，B=I−Σ2(Σ1+Σ2)−1B=I-\Sigma_2(\Sigma_1+\Sigma_2)^{-1}.

带入u\mathrm{u}和Σ\mathrm{\Sigma}的表达式得

u=Au1+Bu2=u2+A(u1−u2)=u2+Σ2(Σ1+Σ2)−1(u1−u2)\begin{aligned} \mathrm{u}&=A\mathrm{u}_1+B\mathrm{u}_2\\ &=\mathrm{u}_2+A(\mathrm{u}_1-\mathrm{u}_2)\\ &=\mathrm{u}_2+\Sigma_2(\Sigma_1+\Sigma_2)^{-1}(\mathrm{u}_1-\mathrm{u}_2) \end{aligned}
Σ=AΣ1AT+BΣ2BT=AΣ1AT+(I−A)Σ2(I−A)T=A(Σ1+Σ2)AT+Σ2−AΣ2−Σ2AT=Σ2AT+Σ2−AΣ2−Σ2AT=Σ2−Σ2(Σ1+Σ2)−1Σ2\begin{aligned} \Sigma&=A\Sigma_1A^T+B\Sigma_2B^T\\ &=A\Sigma_1A^T+(I-A)\Sigma_2(I-A)^T\\ &=A(\Sigma_1+\Sigma_2)A^T+\Sigma_2-A\Sigma_2-\Sigma_2A^T\\ &=\Sigma_2A^T+\Sigma_2-A\Sigma_2-\Sigma_2A^T\\ &=\Sigma_2-\Sigma_2(\Sigma_1+\Sigma_2)^{-1}\Sigma_2 \end{aligned}
avatar
文章
阅读
粉丝