其中 $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_kH$ ，则可消除广义逆矩阵 $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

即要使得估计误差最小:

\mathbf{e_k}=\mathbf{x}_k-\hat{\mathbf{{x}}}_k \\ \mathbf{e_k} \to 0

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

\begin{align*} \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(\begin{bmatrix}e_1 \\ e_2\end{bmatrix}\begin{bmatrix}e_1 & e_2\end{bmatrix}) \\ &= \begin{bmatrix} \sigma_{e_1}^2 & \sigma_{e_1}\sigma_{e_2} \\ \sigma_{e_2}\sigma_{e_1} & \sigma_{e_2}^2 \end{bmatrix} \\ \text{tr}(P_k) &= \text{tr}(\begin{bmatrix} \sigma_{e_1}^2 & \sigma_{e_1}\sigma_{e_2} \\ \sigma_{e_2}\sigma_{e_1} & \sigma_{e_2}^2 \end{bmatrix}) \\ &= \text{tr}(\begin{bmatrix} \sigma_{e_1}^2 & 0 \\ 0 & \sigma_{e_2}^2 \end{bmatrix}) \\ &= \sigma_{e_1}^2 + \sigma_{e_2}^2 \\ \end{align*} \\

\begin{align*} \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{align*}

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

\begin{align*} d\frac{\text{tr}(\text{Var}(\mathbf{e_k}))}{dK_k} = 0 \\ \end{align*}

其中

\begin{align*} \mathbf{e_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$)}\\ &= \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$)}\\ &= \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_kH\hat{\mathbf{x}}^-_k & \text{(去除括号)}\\ &= \mathbf{x}_k-\hat{\mathbf{x}}^-_{k} - K_kH\mathbf{x}_k - K_k\mathbf{v}_k+ K_kH\hat{\mathbf{x}}^-_k & \text{(去除括号)}\\ &= (\mathbf{x}_k-\hat{\mathbf{x}}^-_{k}) +(- K_kH\mathbf{x}_k + K_kH\hat{\mathbf{x}}^-_k ) - K_k\mathbf{v}_k & \text{(添加括号)}\\ &= (\mathbf{x}_k-\hat{\mathbf{x}}^-_{k}) - K_kH(\mathbf{x}_k - \hat{\mathbf{x}}^-_k ) - K_k\mathbf{v}_k & \text{(提取$K_kH$)}\\ &= [(\mathbf{x}_k-\hat{\mathbf{x}}^-_{k}) - K_kH(\mathbf{x}_k - \hat{\mathbf{x}}^-_k )] - K_k\mathbf{v}_k & \text{(添加括号)}\\ &= (I - K_kH)(\mathbf{x}_k-\hat{\mathbf{x}}^-_{k}) - K_k\mathbf{v}_k & \text{(提取$\mathbf{x}_k-\hat{\mathbf{x}}^-_{k}$)}\\ &= (I - K_kH)\mathbf{e^-_k} - K_k\mathbf{v}_k &\text{(代入$\mathbf{e^-_k}$)}\\ \end{align*} \\ \text{定义:} \text{先验估计误差}\mathbf{e^-_k}=\mathbf{x}_k-\hat{\mathbf{x}}^-_{k}

转置性质
$(AB)^T=B^TA^T$
$(A+B)^T=A^T+B^T$

其中

\begin{align*} P_k &=\text{Var}(\mathbf{e_k})\\ &= E[e_ke_k^T] \\ &= E[[(I - K_kH)\mathbf{e^-_k} - K_k\mathbf{v}_k ][(I - K_kH)\mathbf{e^-_k} - K_k\mathbf{v}_k ]^T] & \text{(转置性质)}\\ &= E[[(I - K_kH)\mathbf{e^-_k} - K_k\mathbf{v}_k ][\mathbf{e^-_k}^T(I - K_kH)^T - \mathbf{v}_k^TK_k^T ]] & \text{(展开)} \\ &= E[[(I - K_kH)\mathbf{e^-_k}][\mathbf{e^-_k}^T(I - K_kH)^T]-[(I - K_kH)\mathbf{e^-_k}][\mathbf{v}_k^TK_k^T]-[K_k\mathbf{v}_k][\mathbf{e^-_k}^T(I - K_kH)^T]+[K_k\mathbf{v}_k][\mathbf{v}_k^TK_k^T]] & \text{(展开)} \\ &= E[[(I - K_kH)\mathbf{e^-_k}][\mathbf{e^-_k}^T(I - K_kH)^T]]-E[[(I - K_kH)\mathbf{e^-_k}][\mathbf{v}_k^TK_k^T]]-E[[K_k\mathbf{v}_k][\mathbf{e^-_k}^T(I - K_kH)^T]]+E[[K_k\mathbf{v}_k][\mathbf{v}_k^TK_k^T]] & \text{(展开)} \\ &= \text{第1项}-\text{第2项}-\text{第3项}+\text{第4项} \\ \end{align*} \\

其中:

\begin{align*} \text{第1项} &= E[[(I - K_kH)\mathbf{e^-_k}][\mathbf{e^-_k}^T(I - K_kH)^T]] \\ &= (I - K_kH)E[\mathbf{e^-_k}\mathbf{e^-_k}^T](I - K_kH)^T &\text{(提取常数项)}\\ &= (I - K_kH)E[\mathbf{e^-_k}\mathbf{e^-_k}^T](I - K_kH)^T &\text{(代入：$P_k=E(\mathbf{e_k}\mathbf{e_k}^T)$)}\\ &= (I - K_kH)P^-_k(I - K_kH)^T \\ &= (P^-_k - K_kHP^-_k)(I^T - H^TK_k^T) \\ &= (P^-_k - K_kHP^-_k)(I - H^TK_k^T) \\ &= P^-_kI - P^-_kH^TK_k^T - K_kHP^-_kI + K_kHP^-_kH^TK_k^T \\ &= P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + K_kHP^-_kH^TK_k^T \\ \text{第2项} &= E[[(I - K_kH)\mathbf{e^-_k}][\mathbf{v}_k^TK_k^T]] \\ &= (I - K_kH)E[\mathbf{e^-_k}\mathbf{v}_k^T]K_k^T &\text{(提取常数项)}\\ &= (I - K_kH)E[\mathbf{e^-_k}]E[\mathbf{v}_k^T]K_k^T &\text{(估计误差和测量误差相互独立)}\\ &= (I - K_kH) \times 0 \times 0 \times K_k^T &\text{两误差期望为0}\\ &= 0 \\ \text{第3项} &= E[[K_k\mathbf{v}_k][\mathbf{e^-_k}^T(I - K_kH)^T]] \\ &= K_kE[\mathbf{v}_k\mathbf{e^-_k}^T](I - K_kH)^T &\text{(提取常数项)}\\ &= K_kE[\mathbf{v}_k]E[\mathbf{e^-_k}^T](I - K_kH)^T &\text{(估计误差和测量误差相互独立)}\\ &= K_k\times 0 \times 0 \times (I - K_kH)^T &\text{(两误差期望为0)}\\ &= 0 \\ \text{第4项} &= E[[K_k\mathbf{v}_k][\mathbf{v}_k^TK_k^T]] &\text{(提取常数项)}\\ &= K_kE[\mathbf{v}_k\mathbf{v}_k^T]K_k^T &\text{(代入：$R=E(\mathbf{v_k}\mathbf{v_k}^T)$)}\\ &= K_kRK_k^T\\ \end{align*} \\

所以

\begin{align*} P_k &=\text{Var}(\mathbf{e_k})\\ &= \text{第1项}-\text{第2项}-\text{第3项}+\text{第4项} \\ &= (P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + K_kHP^-_kH^TK_k^T) - 0 - 0 + (K_kRK_k^T) \\ &= P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + K_kHP^-_kH^TK_k^T + K_kRK_k^T \\ &= \text{第A项} - \text{第B项} - \text{第C项} + \text{第D项} + \text{第E项} \\ \end{align*} \\

其中

(\text{第B项})^T =(P^-_kH^TK_k^T)^T = ((P^-_kH^T)K_k^T)^T = K_k(P^-_kH^T)^T = K_kHP^-_k = \text{第C项}

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

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

所以

\begin{align*} \text{tr}(P_k) &= \text{tr}(\text{Var}(\mathbf{e_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{align*} \\

由于

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

举例

A=\begin{bmatrix}a_{1,1} & a_{1,2} \\a_{2,1} & a_{2,2}\\\end{bmatrix} \\ B=\begin{bmatrix}b_{1,1} & b_{1,2} \\b_{2,1} & b_{2,2}\\\end{bmatrix} \\ \begin{align*} \text{tr}(AB) &=\text{tr}(\begin{bmatrix}a_{1,1} & a_{1,2} \\a_{2,1} & a_{2,2}\\\end{bmatrix}\begin{bmatrix}b_{1,1} & b_{1,2} \\b_{2,1} & b_{2,2}\\\end{bmatrix}) \\ &=\text{tr}(\begin{bmatrix}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{bmatrix}) \\ &=(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\begin{bmatrix}a_{1,1} & a_{1,2} \\a_{2,1} & a_{2,2}\\\end{bmatrix}} \\ &=\begin{bmatrix}\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{bmatrix} \\ &=\begin{bmatrix}b_{1,1} & b_{2,1} \\ b_{1,2} & b_{2,2}\end{bmatrix} \\ &=B^T \end{align*}

由于

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

举例

A=\begin{bmatrix}a_{1,1} & a_{1,2} \\a_{2,1} & a_{2,2}\\\end{bmatrix} \\ B=\begin{bmatrix}b_{1,1} & b_{1,2} \\b_{2,1} & b_{2,2}\\\end{bmatrix} \\ \begin{align*} \text{tr}(ABA^T) &=\text{tr}( \begin{bmatrix}a_{1,1} & a_{1,2} \\ a_{2,1} & a_{2,2}\\ \end{bmatrix} \begin{bmatrix}b_{1,1} & b_{1,2} \\ b_{2,1} & b_{2,2}\\ \end{bmatrix} \begin{bmatrix}a_{1,1} & a_{1,2} \\ a_{2,1} & a_{2,2}\\ \end{bmatrix}^T ) \\ &=\text{tr}( \begin{bmatrix}a_{1,1} & a_{1,2} \\ a_{2,1} & a_{2,2}\\ \end{bmatrix} \begin{bmatrix}b_{1,1} & b_{1,2} \\ b_{2,1} & b_{2,2}\\ \end{bmatrix} \begin{bmatrix}a_{1,1} & a_{2,1} \\ a_{1,2} & a_{2,2}\\ \end{bmatrix} ) \\ &=\text{tr}( \begin{bmatrix}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{bmatrix} \begin{bmatrix}a_{1,1} & a_{2,1} \\ a_{1,2} & a_{2,2}\\ \end{bmatrix} ) \\ &=\text{tr}( \begin{bmatrix}(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{bmatrix} ) \\ &= (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}(ABA^T)}{dA} &= d\frac{\text{tr}(ABA^T)} {d\begin{bmatrix} a_{1,1} & a_{1,2} \\ a_{2,1} & a_{2,2}\\ \end{bmatrix}} \\ &=\begin{bmatrix} \partial\frac{\text{tr}(ABA^T)}{\partial a_{1,1}} & \partial\frac{\text{tr}(ABA^T)}{\partial a_{1,2}} \\ \partial\frac{\text{tr}(ABA^T)}{\partial a_{2,1}} & \partial\frac{\text{tr}(ABA^T)}{\partial a_{2,2}}\\ \end{bmatrix} \\ &=\begin{bmatrix} 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{bmatrix} \\ &=\begin{bmatrix} 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{bmatrix} \\ &=\begin{bmatrix} 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{bmatrix} +\begin{bmatrix} 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{bmatrix} \\ &= AB+AB^T \\ &= A(B+B^T) \\ \end{align*}

所以

\begin{align*} d\frac{\text{tr}(\text{第A项})}{dK_k} &= d\frac{\text{tr}(P^-_k)}{dK_k} = 0 \\ d\frac{\text{tr}(\text{第C项})}{dK_k} &= d\frac{\text{tr}(K_kHP^-_k)}{dK_k} \\ &= (HP^-_k)^T \\ &= {P^-_k}^TH^T \\ &= P^-_kH^T \\ d\frac{\text{tr}(\text{第D项})}{dK_k} &= d\frac{\text{tr}(K_kHP^-_kH^TK_k^T)}{dK_k} \\ &= K_k(HP^-_kH^T+(HP^-_kH^T)^T) \\ &= K_k(HP^-_kH^T+((H^T)^T(P^-_k)^TH^T)) \\ &= K_k(HP^-_kH^T+HP^-_kH^T) \\ &= 2K_k(HP^-_kH^T) \\ d\frac{\text{tr}(\text{第E项})}{dK_k} &= d\frac{\text{tr}(K_kRK_k^T)}{dK_k} \\ &= K_k(R+R^T) \\ &= K_k(R+R) \\ &= 2K_kR \\ \end{align*}

所以

\begin{align*} d\frac{\text{tr}(P_k)}{dK_k} &= 0 \\ d\frac{\text{tr}(\text{Var}(\mathbf{e_k}))}{dK_k} &= 0 \\ d\frac{\text{tr}(\text{第A项}) - 2\text{tr}(\text{第C项}) + \text{tr}(\text{第D项}) + \text{tr}(\text{第E项})}{dK_k} &= 0 \\ d\frac{\text{tr}(\text{第A项})}{dK_k} - 2d\frac{\text{tr}(\text{第C项})}{dK_k} + d\frac{\text{tr}(\text{第D项})}{dK_k} + d\frac{\text{tr}(\text{第E项})}{dK_k} &= 0 \\ 0 - 2P^-_kH^T + 2K_k(HP^-_kH^T) + 2K_kR &= 0 \\ -P^-_kH^T + K_k(HP^-_kH^T) + K_kR &= 0 \\ K_k(HP^-_kH^T) + K_kR &= P^-_kH^T \\ K_k(HP^-_kH^T + R) &= P^-_kH^T \\ K_k &= \frac{P^-_kH^T}{HP^-_kH^T+R} \\ \end{align*}

所以最终得到：

K_k = \frac{P^-_kH^T}{HP^-_kH^T+R}

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