跳至主要內容

卡尔曼滤波器:误差协方差矩阵

丁毅桂大约 5 分钟

卡尔曼滤波器学习笔记

【卡尔曼滤波器】_DR_CAN合集学习笔记

误差协方差矩阵

对于如下状态空间方程：

{xk=Axk−1+Buk−1+wk−1zk=Hxk+vk \begin{cases} \mathbf{x}_{k} &= A \mathbf{x}_{k-1} + B \mathbf{u}_{k-1} + \mathbf{w}_{k-1} \\ \mathbf{z}_k &= H\mathbf{x}_k + \mathbf{v}_k \\ \end{cases}

其中包含的过程噪声w\mathbf{w}和测量噪声v\mathbf{v}符合正态分布:

w∼N(μ,Q)μ=0;Q=E(wwT)v∼N(μ,R)μ=0;R=E(vvT) \begin{align*} \mathbf{w} &\sim \mathcal{N}(\mu,Q) & \mu=0;Q= E(\mathbf{w}\mathbf{w}^T)\\ \mathbf{v} &\sim \mathcal{N}(\mu,R) & \mu=0;R= E(\mathbf{v}\mathbf{v}^T)\\ \end{align*} \\

定义先验估计为：

x^k−=Ax^k−1+Buk−1 \hat{\mathbf{x}}^-_k = A\hat{\mathbf{x}}_{k-1} + B\mathbf{u}_{k-1}

后验估计为：

x^k=x^k−+Kk(zk−Hx^k−) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}^-_{k} + K_k(\mathbf{z}_k-H\hat{\mathbf{x}}^-_k)

卡尔曼增益为：

Kk=Pk−HTHPk−HT+R K_k = \frac{P^-_kH^T}{HP^-_kH^T+R}

先验误差协方差矩阵（priori error covariance matrix）

上述所有公式中，唯有先验估计误差的方差PK−P^-_K是未知的，于是根据公式可依次推导：

ek−=xk−xk−=(Axk−1+Buk−1+wk−1)−(Ax^k−1+Buk−1)(根据定义代入)=Axk−1+Buk−1+wk−1−Ax^k−1−Buk−1=A(xk−1−x^k−1)+wk−1=Aek−1+wk−1(估计误差e=x−x^) \begin{align*} e^-_k &= \mathbf{x}_k - \mathbf{x}^-_k \\ &= (A \mathbf{x}_{k-1} + B \mathbf{u}_{k-1} + \mathbf{w}_{k-1}) - (A\hat{\mathbf{x}}_{k-1} + B\mathbf{u}_{k-1}) & \text{(根据定义代入)} \\ &= A \mathbf{x}_{k-1} + B \mathbf{u}_{k-1} + \mathbf{w}_{k-1} - A\hat{\mathbf{x}}_{k-1} - B\mathbf{u}_{k-1} \\ &= A(\mathbf{x}_{k-1}-\hat{\mathbf{x}}_{k-1}) + \mathbf{w}_{k-1} \\ &= Ae_{k-1} + \mathbf{w}_{k-1} &\text{(估计误差$e = \mathbf{x}-\hat{\mathbf{x}}$)} \\ \end{align*}

所以：

PK−=E(ek−ek−T)=E[(Aek−1+wk−1)(Aek−1+wk−1)T]=E[(Aek−1+wk−1)(ek−1TAT+wk−1T)]=E[Aek−1ek−1TAT+Aek−1wk−1T+wk−1ek−1TAT+wk−1wk−1T]=E[Aek−1ek−1TAT]+E[Aek−1wk−1T]+E[wk−1ek−1TAT]+E[wk−1wk−1T]=第一项+第二项+第三项+第四项 \begin{align*} P^-_K &= \text{E}(e^-_k{e^-_k}^T) \\ &= \text{E}[(Ae_{k-1} + \mathbf{w}_{k-1})(Ae_{k-1} + \mathbf{w}_{k-1})^T]\\ &= \text{E}[(Ae_{k-1} + \mathbf{w}_{k-1})(e_{k-1}^TA^T + \mathbf{w}_{k-1}^T)]\\ &= \text{E}[Ae_{k-1}e_{k-1}^TA^T + Ae_{k-1}\mathbf{w}_{k-1}^T+ \mathbf{w}_{k-1}e_{k-1}^TA^T+\mathbf{w}_{k-1} \mathbf{w}_{k-1}^T]\\ &= \text{E}[Ae_{k-1}e_{k-1}^TA^T]+\text{E}[Ae_{k-1}\mathbf{w}_{k-1}^T]+ \text{E}[\mathbf{w}_{k-1}e_{k-1}^TA^T]+\text{E}[\mathbf{w}_{k-1}\mathbf{w}_{k-1}^T]\\ &= \text{第一项}+\text{第二项}+\text{第三项}+\text{第四项}\\ \end{align*}

其中

第一项=E[Aek−1ek−1TAT]=AE[ek−1ek−1T]AT=APk−1AT第二项=E[Aek−1wk−1T]=AE[ek−1wk−1T]=AE[(xk−1−x^k−1)wk−1T](ek−1=xk−1−x^k−1)=AE[((Axk−2+Buk−2+wk−2)−x^k−1)wk−1T](xk−1=Axk−2+Buk−2+wk−2)=AE[ek−1]E[wk−1T](ek−1中包含的是wk−2,其和wk−1相互独立)=A×0×0=0第三项=E[wk−1ek−1TAT]=E[wk−1ek−1T]AT=E[wk−1]E[ek−1T]AT(ek−1中包含的是wk−2,其和wk−1相互独立)=0×0×AT=0第四项=E[wk−1wk−1T]=Qk−1(代入)=Q(过程噪声是模型的固有特性，不变) \begin{align*} \text{第一项} &= \text{E}[Ae_{k-1}e_{k-1}^TA^T]\\ &= A\text{E}[e_{k-1}e_{k-1}^T]A^T\\ &=AP_{k-1}A^T\\ \text{第二项} &=\text{E}[Ae_{k-1}\mathbf{w}_{k-1}^T]\\ &=A\text{E}[e_{k-1}\mathbf{w}_{k-1}^T]\\ &=A\text{E}[(\mathbf{x}_{k-1}-\hat{\mathbf{x}}_{k-1})\mathbf{w}_{k-1}^T]&\text{($e_{k-1}=\mathbf{x}_{k-1}-\hat{\mathbf{x}}_{k-1}$)}\\ &=A\text{E}[((A \mathbf{x}_{k-2} + B \mathbf{u}_{k-2} + \mathbf{w}_{k-2})-\hat{\mathbf{x}}_{k-1})\mathbf{w}_{k-1}^T]&\text{($\mathbf{x}_{k-1}=A \mathbf{x}_{k-2} + B \mathbf{u}_{k-2} + \mathbf{w}_{k-2}$)}\\ &=A\text{E}[e_{k-1}]\text{E}[\mathbf{w}_{k-1}^T]&\text{($e_{k-1}$中包含的是$\mathbf{w}_{k-2}$,其和$\mathbf{w}_{k-1}$相互独立)}\\ &=A\times 0 \times 0\\ &=0\\ \text{第三项} &=\text{E}[\mathbf{w}_{k-1}e_{k-1}^TA^T]\\ &=\text{E}[\mathbf{w}_{k-1}e_{k-1}^T]A^T\\ &=\text{E}[\mathbf{w}_{k-1}]\text{E}[e_{k-1}^T]A^T&\text{($e_{k-1}$中包含的是$\mathbf{w}_{k-2}$,其和$\mathbf{w}_{k-1}$相互独立)}\\ &=0 \times 0 \times A^T\\ &=0\\ \text{第四项} &=\text{E}[\mathbf{w}_{k-1}\mathbf{w}_{k-1}^T]\\ &=Q_{k-1} & \text{(代入)}\\ &=Q & \text{(过程噪声是模型的固有特性，不变)} \end{align*}

所以最终得到：

PK−=第一项+第二项+第三项+第四项=APk−1AT+0+0+Q=APk−1AT+Q \begin{align*} P^-_K &= \text{第一项}+\text{第二项}+\text{第三项}+\text{第四项}\\ &=AP_{k-1}A^T+0+0+Q\\ &=AP_{k-1}A^T+Q\\ \end{align*}

后验误差协方差矩阵（posteriori error covariance matrix）

其中的后验估计误差协方差Pk−1P_{k-1}为：

Pk=Var(ek)=Pk−−Pk−HTKkT−KkHPk−+KkHPk−HTKkT+KkRKkT(之前推导的)=Pk−−Pk−HTKkT−KkHPk−+Kk(HPk−HT+R)KkT(提取)=Pk−−Pk−HTKkT−KkHPk−+(Pk−HTHPk−HT+R)(HPk−HT+R)KkT(代入Kk)=Pk−−Pk−HTKkT−KkHPk−+(Pk−HT)KkT(乘除抵消)=Pk−−KkHPk−(第二、四项抵消)=(I−KkH)Pk− \begin{align*} P_k &=\text{Var}(\mathbf{e_k})\\ &= P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + K_kHP^-_kH^TK_k^T + K_kRK_k^T &\text{(之前推导的)}\\ &= P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + K_k(HP^-_kH^T + R)K_k^T &\text{(提取)}\\ &= P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + (\frac{P^-_kH^T}{HP^-_kH^T+R})(HP^-_kH^T + R)K_k^T &\text{(代入$K_k$)}\\ &= P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + (P^-_kH^T)K_k^T &\text{(乘除抵消)}\\ &= P^-_k - K_kHP^-_k &\text{(第二、四项抵消)}\\ &= (I-K_kH)P^-_k \end{align*} \\

总结

状态空间方程：

{xk=Axk−1+Buk−1+wk−1zk=Hxk+vk \begin{cases} \mathbf{x}_{k} &= A \mathbf{x}_{k-1} + B \mathbf{u}_{k-1} + \mathbf{w}_{k-1} \\ \mathbf{z}_k &= H\mathbf{x}_k + \mathbf{v}_k \\ \end{cases}

其中包含的过程噪声w\mathbf{w}和测量噪声v\mathbf{v}符合正态分布:

w∼N(μ,Q)μ=0;Q=E(wwT)v∼N(μ,R)μ=0;R=E(vvT) \begin{align*} \mathbf{w} &\sim \mathcal{N}(\mu,Q) & \mu=0;Q= E(\mathbf{w}\mathbf{w}^T)\\ \mathbf{v} &\sim \mathcal{N}(\mu,R) & \mu=0;R= E(\mathbf{v}\mathbf{v}^T)\\ \end{align*} \\

第一步：求先验估计

不考虑过程噪声，利用上次状态，计算预测本次状态（先验估计）

x^k−=Ax^k−1+Buk−1 \hat{\mathbf{x}}^-_k = A\hat{\mathbf{x}}_{k-1} + B\mathbf{u}_{k-1}

第二步：求先验估计误差（的协方差矩阵）

根据上次后验估计误差（的协方差矩阵）和模型误差（过程噪声的协方差矩阵），求本次先验误差（的协方差矩阵）

PK−=APk−1AT+Q P^-_K = AP_{k-1}A^T+Q\\

第三步：求卡尔曼增益：

根据估计误差和测量误差，求数据融合的最优系数

Kk=Pk−HTHPk−HT+R K_k = \frac{P^-_kH^T}{HP^-_kH^T+R}

第四步：求后验估计：

通过卡尔曼增益，将估计值和测量值数据融合，得到后验最优估计

x^k=x^k−+Kk(zk−Hx^k−) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}^-_{k} + K_k(\mathbf{z}_k-H\hat{\mathbf{x}}^-_k)

第五步：求后验估计误差（的协方差矩阵）：

根据本次卡尔曼增益和先验估计误差，求本次后验估计的误差

Pk=(I−KkH)Pk− \begin{align*} P_k = (I-K_kH)P^-_k \end{align*} \\

贡献者: YiguiDing
MIT Licensed | Copyright © 2019-present Yigui-Ding