卡尔曼滤波器:卡尔曼增益推导

卡尔曼滤波器学习笔记

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

卡尔曼增益推导

状态空间方程和噪声

对于一个经过离散化的包含过程噪声和测量噪声的状态空间方程：

$$\{\begin{array}{ll}{\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{array}$$

其中过程噪声$\mathbf{w}$符合正态分布:

$$\begin{array}{rl}\mathbf{w}& \sim \mathcal{N}(\mu ,Q)\\ \mu & =E(\mathbf{w})=0\\ Q& =\text{Var}(\mathbf{w})\\ & =E({\mathbf{w}}^2)-E^2(\mathbf{w})\\ & =E({\mathbf{w}}^2)-0\\ & =E(\mathbf{w}{\mathbf{w}}^T)\end{array}$$

其中测量噪声$\mathbf{v}$符合正态分布:

$$\begin{array}{rl}\mathbf{v}& \sim \mathcal{N}(\mu ,R)\\ \mu & =E(\mathbf{v})=0\\ R& =\text{Var}(\mathbf{v})\\ & =E({\mathbf{v}}^2)-E^2(\mathbf{v})\\ & =E({\mathbf{v}}^2)-0\\ & =E(\mathbf{v}{\mathbf{v}}^T)\end{array}$$

举例：

$$\begin{array}{rl}\mathbf{w}& =\left[\begin{array}{c}{w}_1\\ w_2\end{array}\right]\\ \\ \text{Var}(\mathbf{w})& =E(\mathbf{w}{\mathbf{w}}^T)\\ & =E(\left[\begin{array}{c}{w}_1\\ w_2\end{array}\right]\left[\begin{array}{cc}{w}_1& w_2\end{array}\right])\\ & =E(\left[\begin{array}{cc}{w}_1{w}_1& w_1{w}_2\\ w_2{w}_1& w_2{w}_2\end{array}\right])\\ & =\left[\begin{array}{cc}E(w_1{w}_1)& E(w_1{w}_2)\\ E(w_2{w}_1)& E(w_2{w}_2)\end{array}\right]\\ & =\left[\begin{array}{cc}E(w_1^2)& E(w_1{w}_2)\\ E(w_2{w}_1)& E(w_2^2)\end{array}\right]\\ & =\left[\begin{array}{cc}\text{Var}(w_1)+E^2(w_1)& \text{Cov}(w_1,w_2)+E(w_1)E(w_2)\\ \text{Cov}(w_2,w_1)+E(w_2)E(w_1)& \text{Var}(w_2)+E^2(w_2)\end{array}\right]\\ & =\left[\begin{array}{cc}\text{Var}(w_1)+0& \text{Cov}(w_1,w_2)+0\\ \text{Cov}(w_2,w_1)+0& \text{Var}(w_2)+0\end{array}\right]\\ & =\left[\begin{array}{cc}\text{Var}(w_1)& \text{Cov}(w_1,w_2)\\ \text{Cov}(w_2,w_1)& \text{Var}(w_2)\end{array}\right]& \text{(协方差矩阵)}\\ & =\left[\begin{array}{cc}{\sigma }_{w_1}^2& {\sigma }_{w_1}{\sigma }_{w_2}\\ {\sigma }_{w_2}{\sigma }_{w_1}& {\sigma }_{w_2}^2\end{array}\right]\end{array}$$

先验估计和测量估计

忽略模型的过程噪声，根据上一时刻状态(后验估计)，计算下一时刻状态，定义为先验估计：

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

忽略传感器的测量噪声，根据传感器测量得到的状态Z,反推状态X,定义为测量估计：

$${{\hat{\mathbf{x}}}_k}_{MES}=H^{-}{\mathbf{z}}_k$$

数据融合

上述两个状态，

• 前者为忽略过程噪声利用模型对X的预测，
• 后者为忽略测量噪声利用传感器对X的测量，

利用卡尔曼滤波器对两者进行数据融合（Data Fusion）,得到后验估计：

$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+G(H^{-}{\mathbf{z}}_k-{\hat{\mathbf{x}}}_k^{-})$$

其中$G\in [0,1]$

• 当 $G\to 0$ 时，${\hat{\mathbf{x}}}_k\to {\hat{\mathbf{x}}}_k^{-}$ 即: 估计值 -> 预测值
• 当 $G\to 1$ 时，${\hat{\mathbf{x}}}_k\to {{\hat{\mathbf{x}}}_k}_{MES}$ 即: 估计值 -> 测量值

如若定义$G=K_{k}H$，则可消除广义逆矩阵$N^{-}$：

$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+K_k({\mathbf{z}}_k-H{\hat{\mathbf{x}}}_k^{-})$$

其中$K_k\in [0,H^{-}]$

• 当 $K_k\to 0$ 时，${\hat{\mathbf{x}}}_k\to {\hat{\mathbf{x}}}_k^{-}$ 即: 估计值 -> 预测值
• 当 $K_k\to H^{-}$ 时，${\hat{\mathbf{x}}}_k\to {{\hat{\mathbf{x}}}_k}_{MES}$ 即: 估计值 -> 测量值

卡尔曼增益K

要找到一个$K_k$使得估计值尽可能接近实际值，即：

$${\hat{\mathbf{x}}}_k\to {\mathbf{x}}_k$$

即要使得估计误差最小:

$$\begin{gathered} {\mathbf{e}}_{\mathbf{k}}={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k \\ {\mathbf{e}}_{\mathbf{k}}\to 0 \end{gathered}$$

既要使得估计误差的方差最小、协方差矩阵最小、协方差矩阵的迹最小：

$$\begin{array}{rl}{\mathbf{e}}_k& \sim \mathcal{N}(\mu ,P)\\ \mu & =E({\mathbf{e}}_k)=0\\ P_k& =\text{Var}({\mathbf{e}}_k)\\ & =E({\mathbf{e}}_k{\mathbf{e}}_k^T)\\ & =E(\left[\begin{array}{c}{e}_1\\ e_2\end{array}\right]\left[\begin{array}{cc}{e}_1& e_2\end{array}\right])\\ & =\left[\begin{array}{cc}{\sigma }_{e_1}^2& {\sigma }_{e_1}{\sigma }_{e_2}\\ {\sigma }_{e_2}{\sigma }_{e_1}& {\sigma }_{e_2}^2\end{array}\right]\\ \text{tr}(P_k)& =\text{tr}(\left[\begin{array}{cc}{\sigma }_{e_1}^2& {\sigma }_{e_1}{\sigma }_{e_2}\\ {\sigma }_{e_2}{\sigma }_{e_1}& {\sigma }_{e_2}^2\end{array}\right])\\ & =\text{tr}(\left[\begin{array}{cc}{\sigma }_{e_1}^2& 0\\ 0& {\sigma }_{e_2}^2\end{array}\right])\\ & ={\sigma }_{e_1}^2+{\sigma }_{e_2}^2\end{array}$$$$\begin{array}{rl}{\mathbf{e}}_k& \to 0;\\ P_k=\text{Var}({\mathbf{e}}_k)& \to 0;\\ \text{tr}(P_k)=\text{tr}(\text{Var}({\mathbf{e}}_k))& \to 0;\end{array}$$

总之，就是要找到一个$K_k$，使其估计误差${\mathbf{e}}_{\mathbf{k}}$最小，只需使估计误差的方差的迹$\text{tr}(\text{Var}({\mathbf{e}}_{\mathbf{k}}))$最小，即：

$$\begin{array}{r}d\frac{\text{tr}(\text{Var}({\mathbf{e}}_{\mathbf{k}}))}{d{K}_k}=0\end{array}$$

其中

$$\begin{gathered} \begin{array}{rl}{\mathbf{e}}_{\mathbf{k}}& ={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k\\ & ={\mathbf{x}}_k-[{\hat{\mathbf{x}}}_k^{-}+K_k({\mathbf{z}}_k-H{\hat{\mathbf{x}}}_k^{-})]& \text{(代入}{\hat{\mathbf{x}}}_k\text{)}\\ & ={\mathbf{x}}_k-[{\hat{\mathbf{x}}}_k^{-}+K_k([H{\mathbf{x}}_k+{\mathbf{v}}_k]-H{\hat{\mathbf{x}}}_k^{-})]& \text{(代入}{\mathbf{z}}_k\text{)}\\ & ={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}-K_k([H{\mathbf{x}}_k+{\mathbf{v}}_k]-H{\hat{\mathbf{x}}}_k^{-})& \text{(去除括号)}\\ & ={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}-K_k[H{\mathbf{x}}_k+{\mathbf{v}}_k]+K_{k}H{\hat{\mathbf{x}}}_k^{-}& \text{(去除括号)}\\ & ={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}-K_{k}H{\mathbf{x}}_k-K_k{\mathbf{v}}_k+K_{k}H{\hat{\mathbf{x}}}_k^{-}& \text{(去除括号)}\\ & =({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-})+(-K_{k}H{\mathbf{x}}_k+K_{k}H{\hat{\mathbf{x}}}_k^{-})-K_k{\mathbf{v}}_k& \text{(添加括号)}\\ & =({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-})-K_{k}H({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-})-K_k{\mathbf{v}}_k& \text{(提取}{K}_{k}H\text{)}\\ & =[({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-})-K_{k}H({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-})]-K_k{\mathbf{v}}_k& \text{(添加括号)}\\ & =(I-K_{k}H)({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-})-K_k{\mathbf{v}}_k& \text{(提取}{\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}\text{)}\\ & =(I-K_{k}H){\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}-K_k{\mathbf{v}}_k& \text{(代入}{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}\text{)}\end{array} \\ \text{定义:}\text{先验估计误差}{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-} \end{gathered}$$

转置性质

• $(AB{)}^T=B^T{A}^T$
• $(A+B{)}^T=A^T+B^T$

其中

$$\begin{array}{rl}{P}_k& =\text{Var}({\mathbf{e}}_{\mathbf{k}})\\ & =E[e_k{e}_k^T]\\ & =E[[(I-K_{k}H){\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}-K_k{\mathbf{v}}_k][(I-K_{k}H){\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}-K_k{\mathbf{v}}_k{]}^T]& \text{(转置性质)}\\ & =E[[(I-K_{k}H){\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}-K_k{\mathbf{v}}_k][{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T(I-K_{k}H{)}^T-{\mathbf{v}}_k^T{K}_k^T]]& \text{(展开)}\\ & =E[[(I-K_{k}H){\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}][{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T(I-K_{k}H{)}^T]-[(I-K_{k}H){\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}][{\mathbf{v}}_k^T{K}_k^T]-[K_k{\mathbf{v}}_k][{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T(I-K_{k}H{)}^T]+[K_k{\mathbf{v}}_k][{\mathbf{v}}_k^T{K}_k^T]]& \text{(展开)}\\ & =E[[(I-K_{k}H){\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}][{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T(I-K_{k}H{)}^T]]-E[[(I-K_{k}H){\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}][{\mathbf{v}}_k^T{K}_k^T]]-E[[K_k{\mathbf{v}}_k][{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T(I-K_{k}H{)}^T]]+E[[K_k{\mathbf{v}}_k][{\mathbf{v}}_k^T{K}_k^T]]& \text{(展开)}\\ & =\text{第1项}-\text{第2项}-\text{第3项}+\text{第4项}\end{array}$$

其中:

$$\begin{array}{rl}\text{第1项}& =E[[(I-K_{k}H){\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}][{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T(I-K_{k}H{)}^T]]\\ & =(I-K_{k}H)E[{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T](I-K_{k}H{)}^T& \text{(提取常数项)}\\ & =(I-K_{k}H)E[{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T](I-K_{k}H{)}^T& \text{(代入：}{P}_k=E({\mathbf{e}}_{\mathbf{k}}{{\mathbf{e}}_{\mathbf{k}}}^T)\text{)}\\ & =(I-K_{k}H)P_k^{-}(I-K_{k}H{)}^T\\ & =(P_k^{-}-K_{k}H{P}_k^{-})(I^T-H^T{K}_k^T)\\ & =(P_k^{-}-K_{k}H{P}_k^{-})(I-H^T{K}_k^T)\\ & =P_k^{-}I-P_k^{-}{H}^T{K}_k^T-K_{k}H{P}_k^{-}I+K_{k}H{P}_k^{-}{H}^T{K}_k^T\\ & =P_k^{-}-P_k^{-}{H}^T{K}_k^T-K_{k}H{P}_k^{-}+K_{k}H{P}_k^{-}{H}^T{K}_k^T\\ \text{第2项}& =E[[(I-K_{k}H){\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}][{\mathbf{v}}_k^T{K}_k^T]]\\ & =(I-K_{k}H)E[{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}{\mathbf{v}}_k^T]K_k^T& \text{(提取常数项)}\\ & =(I-K_{k}H)E[{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}]E[{\mathbf{v}}_k^T]K_k^T& \text{(估计误差和测量误差相互独立)}\\ & =(I-K_{k}H)\times 0\times 0\times K_k^T& \text{两误差期望为0}\\ & =0\\ \text{第3项}& =E[[K_k{\mathbf{v}}_k][{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T(I-K_{k}H{)}^T]]\\ & =K_{k}E[{\mathbf{v}}_k{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T](I-K_{k}H{)}^T& \text{(提取常数项)}\\ & =K_{k}E[{\mathbf{v}}_k]E[{{\mathbf{e}}_{\mathbf{k}}^{\mathbf{-}}}^T](I-K_{k}H{)}^T& \text{(估计误差和测量误差相互独立)}\\ & =K_k\times 0\times 0\times (I-K_{k}H{)}^T& \text{(两误差期望为0)}\\ & =0\\ \text{第4项}& =E[[K_k{\mathbf{v}}_k][{\mathbf{v}}_k^T{K}_k^T]]& \text{(提取常数项)}\\ & =K_{k}E[{\mathbf{v}}_k{\mathbf{v}}_k^T]K_k^T& \text{(代入：}R=E({\mathbf{v}}_{\mathbf{k}}{{\mathbf{v}}_{\mathbf{k}}}^T)\text{)}\\ & =K_{k}R{K}_k^T\end{array}$$

所以

$$\begin{array}{rl}{P}_k& =\text{Var}({\mathbf{e}}_{\mathbf{k}})\\ & =\text{第1项}-\text{第2项}-\text{第3项}+\text{第4项}\\ & =(P_k^{-}-P_k^{-}{H}^T{K}_k^T-K_{k}H{P}_k^{-}+K_{k}H{P}_k^{-}{H}^T{K}_k^T)-0-0+(K_{k}R{K}_k^T)\\ & =P_k^{-}-P_k^{-}{H}^T{K}_k^T-K_{k}H{P}_k^{-}+K_{k}H{P}_k^{-}{H}^T{K}_k^T+K_{k}R{K}_k^T\\ & =\text{第A项}-\text{第B项}-\text{第C项}+\text{第D项}+\text{第E项}\end{array}$$

其中

$$(\text{第B项}{)}^T=(P_k^{-}{H}^T{K}_k^T{)}^T=((P_k^{-}{H}^T)K_k^T{)}^T=K_k(P_k^{-}{H}^T{)}^T=K_{k}H{P}_k^{-}=\text{第C项}$$

说明两矩阵对角线元素相同，所以

$$\text{tr}(\text{第B项})=\text{tr}(\text{第C项})$$

所以

$$\begin{array}{rl}\text{tr}(P_k)& =\text{tr}(\text{Var}({\mathbf{e}}_{\mathbf{k}}))\\ & =\text{tr}(\text{第A项}-\text{第B项}-\text{第C项}+\text{第D项}+\text{第E项})\\ & =\text{tr}(\text{第A项})-\text{tr}(\text{第B项})-\text{tr}(\text{第C项})+\text{tr}(\text{第D项})+\text{tr}(\text{第E项})\\ & =\text{tr}(\text{第A项})-2\text{tr}(\text{第C项})+\text{tr}(\text{第D项})+\text{tr}(\text{第E项})\end{array}$$

由于

$$d\frac{\text{tr}(AB)}{dA}=B^T$$

举例

$$\begin{gathered} A=\left[\begin{array}{cc}{a}_{1,1}& a_{1,2}\\ a_{2,1}& a_{2,2}\end{array}\right] \\ B=\left[\begin{array}{cc}{b}_{1,1}& b_{1,2}\\ b_{2,1}& b_{2,2}\end{array}\right] \\ \begin{array}{rl}\text{tr}(AB)& =\text{tr}(\left[\begin{array}{cc}{a}_{1,1}& a_{1,2}\\ a_{2,1}& a_{2,2}\end{array}\right]\left[\begin{array}{cc}{b}_{1,1}& b_{1,2}\\ b_{2,1}& b_{2,2}\end{array}\right])\\ & =\text{tr}(\left[\begin{array}{cc}{a}_{1,1}{b}_{1,1}+a_{1,2}{b}_{2,1}& \\ & a_{2,1}{b}_{1,2}+a_{2,2}{b}_{2,2}\end{array}\right])\\ & =(a_{1,1}{b}_{1,1}+a_{1,2}{b}_{2,1})+(a_{2,1}{b}_{1,2}+a_{2,2}{b}_{2,2})\\ & =a_{1,1}{b}_{1,1}+a_{1,2}{b}_{2,1}+a_{2,1}{b}_{1,2}+a_{2,2}{b}_{2,2}\\ d\frac{\text{tr}(AB)}{dA}& =d\frac{\text{tr}(AB)}{d\left[\begin{array}{cc}{a}_{1,1}& a_{1,2}\\ a_{2,1}& a_{2,2}\end{array}\right]}\\ & =\left[\begin{array}{cc}\partial \frac{\text{tr}(AB)}{\partial a_{1,1}}& \partial \frac{\text{tr}(AB)}{\partial a_{1,2}}\\ \partial \frac{\text{tr}(AB)}{\partial a_{2,1}}& \partial \frac{\text{tr}(AB)}{\partial a_{2,2}}\end{array}\right]\\ & =\left[\begin{array}{cc}{b}_{1,1}& b_{2,1}\\ b_{1,2}& b_{2,2}\end{array}\right]\\ & =B^T\end{array} \end{gathered}$$

由于

$$d\frac{\text{tr}(AB{A}^T)}{dA}=A(B+B^T)$$

举例

$$\begin{gathered} A=\left[\begin{array}{cc}{a}_{1,1}& a_{1,2}\\ a_{2,1}& a_{2,2}\end{array}\right] \\ B=\left[\begin{array}{cc}{b}_{1,1}& b_{1,2}\\ b_{2,1}& b_{2,2}\end{array}\right] \\ \begin{array}{rl}\text{tr}(AB{A}^T)& =\text{tr}(\left[\begin{array}{cc}{a}_{1,1}& a_{1,2}\\ a_{2,1}& a_{2,2}\end{array}\right]\left[\begin{array}{cc}{b}_{1,1}& b_{1,2}\\ b_{2,1}& b_{2,2}\end{array}\right]{\left[\begin{array}{cc}{a}_{1,1}& a_{1,2}\\ a_{2,1}& a_{2,2}\end{array}\right]}^T)\\ & =\text{tr}(\left[\begin{array}{cc}{a}_{1,1}& a_{1,2}\\ a_{2,1}& a_{2,2}\end{array}\right]\left[\begin{array}{cc}{b}_{1,1}& b_{1,2}\\ b_{2,1}& b_{2,2}\end{array}\right]\left[\begin{array}{cc}{a}_{1,1}& a_{2,1}\\ a_{1,2}& a_{2,2}\end{array}\right])\\ & =\text{tr}(\left[\begin{array}{cc}{a}_{1,1}{b}_{1,1}+a_{1,2}{b}_{2,1}& a_{1,1}{b}_{1,2}+a_{1,2}{b}_{2,2}\\ a_{2,1}{b}_{1,1}+a_{2,2}{b}_{2,1}& a_{2,1}{b}_{1,2}+a_{2,2}{b}_{2,2}\end{array}\right]\left[\begin{array}{cc}{a}_{1,1}& a_{2,1}\\ a_{1,2}& a_{2,2}\end{array}\right])\\ & =\text{tr}(\left[\begin{array}{cc}(a_{1,1}{b}_{1,1}+a_{1,2}{b}_{2,1})a_{1,1}+(a_{1,1}{b}_{1,2}+a_{1,2}{b}_{2,2})a_{1,2}& \\ & (a_{2,1}{b}_{1,1}+a_{2,2}{b}_{2,1})a_{2,1}+(a_{2,1}{b}_{1,2}+a_{2,2}{b}_{2,2})a_{2,2}\end{array}\right])\\ & =(a_{1,1}{b}_{1,1}+a_{1,2}{b}_{2,1})a_{1,1}+(a_{1,1}{b}_{1,2}+a_{1,2}{b}_{2,2})a_{1,2}+(a_{2,1}{b}_{1,1}+a_{2,2}{b}_{2,1})a_{2,1}+(a_{2,1}{b}_{1,2}+a_{2,2}{b}_{2,2})a_{2,2}\\ d\frac{\text{tr}(AB{A}^T)}{dA}& =d\frac{\text{tr}(AB{A}^T)}{d\left[\begin{array}{cc}{a}_{1,1}& a_{1,2}\\ a_{2,1}& a_{2,2}\end{array}\right]}\\ & =\left[\begin{array}{cc}\partial \frac{\text{tr}(AB{A}^T)}{\partial a_{1,1}}& \partial \frac{\text{tr}(AB{A}^T)}{\partial a_{1,2}}\\ \partial \frac{\text{tr}(AB{A}^T)}{\partial a_{2,1}}& \partial \frac{\text{tr}(AB{A}^T)}{\partial a_{2,2}}\end{array}\right]\\ & =\left[\begin{array}{cc}2a_{1,1}{b}_{1,1}+a_{1,2}{b}_{2,1}+b_{1,2}{a}_{1,2}& b_{2,1}{a}_{1,1}+a_{1,1}{b}_{1,2}+2a_{1,2}{b}_{2,2}\\ 2a_{2,1}{b}_{1,1}+a_{2,2}{b}_{2,1}+b_{1,2}{a}_{2,2}& b_{2,1}{a}_{2,1}+a_{2,1}{b}_{1,2}+2a_{2,2}{b}_{2,2}\end{array}\right]\\ & =\left[\begin{array}{cc}{a}_{1,1}{b}_{1,1}+(a_{1,1}{b}_{1,1}+a_{1,2}{b}_{2,1})+b_{1,2}{a}_{1,2}& b_{2,1}{a}_{1,1}+(a_{1,1}{b}_{1,2}+a_{1,2}{b}_{2,2})+a_{1,2}{b}_{2,2}\\ a_{2,1}{b}_{1,1}+(a_{2,1}{b}_{1,1}+a_{2,2}{b}_{2,1})+b_{1,2}{a}_{2,2}& b_{2,1}{a}_{2,1}+(a_{2,1}{b}_{1,2}+a_{2,2}{b}_{2,2})+a_{2,2}{b}_{2,2}\end{array}\right]\\ & =\left[\begin{array}{cc}{a}_{1,1}{b}_{1,1}+a_{1,2}{b}_{2,1}& a_{1,1}{b}_{1,2}+a_{1,2}{b}_{2,2}\\ a_{2,1}{b}_{1,1}+a_{2,2}{b}_{2,1}& a_{2,1}{b}_{1,2}+a_{2,2}{b}_{2,2}\end{array}\right]+\left[\begin{array}{cc}{a}_{1,1}{b}_{1,1}+b_{1,2}{a}_{1,2}& b_{2,1}{a}_{1,1}+a_{1,2}{b}_{2,2}\\ a_{2,1}{b}_{1,1}+b_{1,2}{a}_{2,2}& b_{2,1}{a}_{2,1}+a_{2,2}{b}_{2,2}\end{array}\right]\\ & =AB+A{B}^T\\ & =A(B+B^T)\end{array} \end{gathered}$$

所以

$$\begin{array}{rl}d\frac{\text{tr}(\text{第A项})}{d{K}_k}& =d\frac{\text{tr}(P_k^{-})}{d{K}_k}=0\\ d\frac{\text{tr}(\text{第C项})}{d{K}_k}& =d\frac{\text{tr}(K_{k}H{P}_k^{-})}{d{K}_k}\\ & =(H{P}_k^{-}{)}^T\\ & ={P_k^{-}}^T{H}^T\\ & =P_k^{-}{H}^T\\ d\frac{\text{tr}(\text{第D项})}{d{K}_k}& =d\frac{\text{tr}(K_{k}H{P}_k^{-}{H}^T{K}_k^T)}{d{K}_k}\\ & =K_k(H{P}_k^{-}{H}^T+(H{P}_k^{-}{H}^T{)}^T)\\ & =K_k(H{P}_k^{-}{H}^T+((H^T{)}^T(P_k^{-}{)}^T{H}^T))\\ & =K_k(H{P}_k^{-}{H}^T+H{P}_k^{-}{H}^T)\\ & =2K_k(H{P}_k^{-}{H}^T)\\ d\frac{\text{tr}(\text{第E项})}{d{K}_k}& =d\frac{\text{tr}(K_{k}R{K}_k^T)}{d{K}_k}\\ & =K_k(R+R^T)\\ & =K_k(R+R)\\ & =2K_{k}R\end{array}$$

所以

$$\begin{array}{rl}d\frac{\text{tr}(P_k)}{d{K}_k}& =0\\ d\frac{\text{tr}(\text{Var}({\mathbf{e}}_{\mathbf{k}}))}{d{K}_k}& =0\\ d\frac{\text{tr}(\text{第A项})-2\text{tr}(\text{第C项})+\text{tr}(\text{第D项})+\text{tr}(\text{第E项})}{d{K}_k}& =0\\ d\frac{\text{tr}(\text{第A项})}{d{K}_k}-2d\frac{\text{tr}(\text{第C项})}{d{K}_k}+d\frac{\text{tr}(\text{第D项})}{d{K}_k}+d\frac{\text{tr}(\text{第E项})}{d{K}_k}& =0\\ 0-2P_k^{-}{H}^T+2K_k(H{P}_k^{-}{H}^T)+2K_{k}R& =0\\ -P_k^{-}{H}^T+K_k(H{P}_k^{-}{H}^T)+K_{k}R& =0\\ K_k(H{P}_k^{-}{H}^T)+K_{k}R& =P_k^{-}{H}^T\\ K_k(H{P}_k^{-}{H}^T+R)& =P_k^{-}{H}^T\\ K_k& =\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}\end{array}$$

所以最终得到：

$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$

其中

• 测量误差 $R\to +\mathrm{\infty }$ 或 先验估计误差 $P_k^{-}\to 0$
  • 则：$K_k\to \frac{0_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}=0$
• 测量误差 $R\to 0$ 或 先验估计误差 $P_k^{-}\to +\mathrm{\infty }$
  • 则：$K_k\to \frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+0}=\frac{1}{H}=H^{-1}$