- 前言
- 系统建模
- 状态预测
- 最优估计问题
- 卡尔曼增益
- 后记
前两次卡尔曼滤波，分别写了卡尔曼滤波的两大流程（预测、更新）两个方程五个公式，和卡尔曼滤波与贝叶斯滤波的关系。
本次将着重于从概率角度上推导卡尔曼滤波的五个公式，以及卡尔曼增益。
系统建模设线性系统状态 X = ( x 1 , x 2 , . . . … , x n ) X=(x_1,x_2,...\dots,x_n) X=(x1,x2,...…,xn)，上一时刻的状态最优估计是 x ^ n − 1 ∼ N ( μ ^ n − 1 , P ^ n − 1 ) \hat x_{n-1} \sim N(\hat\mu_{n-1},\hat P_{n-1}) x^n−1∼N(μ^n−1,P^n−1)，本时刻的观测状态是 z ^ n \hat z_n z^n。假设系统满足马尔可夫性，求当前时刻的状态最优估计 x ^ n \hat x_n x^n
状态方程与观测方程：（后续不考虑控制输入矩阵） x n = A x n − 1 + B u n + w n , w n ∼ N ( 0 , R ) z n = H x n + v n , v n ∼ N ( 0 , Q ) x_n = Ax_{n-1}+Bu_n+w_n,w_n \sim N(0, R) \\ z_n = Hx_n+v_n,v_n \sim N(0,Q) xn=Axn−1+Bun+wn,wn∼N(0,R)zn=Hxn+vn,vn∼N(0,Q)
状态预测当前时刻的状态预测可以通过状态方程与上一时刻的最优估计获得： x ^ n ∣ n − 1 = A x ^ n − 1 + w n \hat x_{n|n-1} = A\hat x_{n-1} + w_n \\ x^n∣n−1=Ax^n−1+wn 由于 x ^ n − 1 ∼ N ( μ ^ n − 1 , P ^ n − 1 ) \hat x_{n-1} \sim N(\hat\mu_{n-1},\hat P_{n-1}) x^n−1∼N(μ^n−1,P^n−1)，由概率分布期望和协方差矩阵的性质，可知： x ^ n ∣ n − 1 ∼ N ( A μ ^ n − 1 , A P ^ n − 1 A T + R ) \hat x_{n|n-1} \sim N(A\hat \mu_{n-1},A\hat P_{n-1}A^T+R) x^n∣n−1∼N(Aμ^n−1,AP^n−1AT+R) 当期时刻的观测预测可以通过观测方程和状态预测获得： z ^ n ∣ n − 1 = H x ^ n ∣ n − 1 + v n \hat z_{n|n-1}=H \hat x_{n|n-1}+v_n z^n∣n−1=Hx^n∣n−1+vn 由概率分布期望和协方差矩阵的性质可得： z ^ n ∣ n − 1 ∼ N ( H A μ ^ n − 1 , H A P ^ n − 1 A T H T + H R ) \hat z_{n|n-1}\sim N(HA\hat \mu_{n-1}, HA\hat P_{n-1}A^TH^T + HR) z^n∣n−1∼N(HAμ^n−1,HAP^n−1ATHT+HR) 需要注意的是，观测预测没有观测噪声，因为观测噪声是传感器带来的，而预测并不涉及传感器。
最优估计问题本时刻的观测为 z ^ n \hat z_n z^n，观测预测为 z ^ n ∣ n − 1 \hat z_{n|n-1} z^n∣n−1，二者之间的差值就是观测残差： Δ z ^ n = z ^ n − z ^ n ∣ n − 1 \Delta \hat z_n = \hat z_n - \hat z_{n|n-1} \\ Δz^n=z^n−z^n∣n−1
观测残差的意义是，本时刻的观测与预测的偏差，而卡尔曼增益，就是通过预测量与观测量偏差进行加权，获得最优估计： x ^ n = x ^ n ∣ n − 1 + K k Δ z ^ n x ^ n ∼ N ( E ( x ^ n ∣ n − 1 ) + K k E ( Δ z ^ n ) , P ^ n ∣ n − 1 + K k P ^ Δ z ^ n K k T ) \hat x_n = \hat x_{n|n-1}+K_k \Delta \hat z_n \\ \hat x_n \sim N(E(\hat x_{n|n-1})+K_kE(\Delta \hat z_n),\hat P_{n|n-1}+K_k \hat P_{\Delta\hat z_n}K_k^T) x^n=x^n∣n−1+KkΔz^nx^n∼N(E(x^n∣n−1)+KkE(Δz^n),P^n∣n−1+KkP^Δz^nKkT)
此外，卡尔曼增益不仅代表了观测偏差的权，也负责将观测量线性变换到状态量的尺度（这是因为在不同系统中，观测与状态的范围不一定相同）。
现在的问题是，什么是最优估计？或者说，怎样的 x ^ n \hat x_{n} x^n才是 x n x_n xn的最优估计？ 答案是：与 x n x_n xn相差最小的 x ^ n \hat x_{n} x^n就是最优估计，可以用以下公式来衡量： arg min x ^ n E ( ∥ x ^ n − x n ∥ 2 ) \argmin_{\hat x_n} E(\Vert \hat x_n-x_n \Vert^2) x^nargminE(∥x^n−xn∥2) 其中， x n ∈ R n x_n \in R^n xn∈Rn（也就是说真实状态是一个常向量，定值）。
向量范数的最小值问题很难求解，涉及到优化理论。那么将以上式子进行变换： E ( ∥ x ^ n − x n ∥ 2 ) = t r [ E ( x ^ n − x n ) ( x ^ n − x n ) T ] = t r [ C o v ( x ^ n − x n , x ^ n − x n ) ] E(\Vert \hat x_n-x_n \Vert^2)=tr[E(\hat x_n-x_n)(\hat x_n-x_n)^T] \\ = tr[Cov(\hat x_n-x_n, \hat x_n-x_n)] E(∥x^n−xn∥2)=tr[E(x^n−xn)(x^n−xn)T]=tr[Cov(x^n−xn,x^n−xn)] 右边的式子是 x ^ n − x n \hat x_n-x_n x^n−xn的协方差矩阵，协方差矩阵的对角线元素就是 x ^ n − x n \hat x_n-x_n x^n−xn各分量的方差，这里以分量 x 1 为 例 x_1为例 x1为例： V a r ( x ^ 1 − x 1 ) = E ( x ^ 1 − x 1 − E ( x ^ 1 − x 1 ) ) 2 = E ( x ^ 1 − x 1 ) 2 Var(\hat x_1-x_1)=E(\hat x_1-x_1 - E(\hat x_1-x_1))^2\\ = E(\hat x_1-x_1)^2 Var(x^1−x1)=E(x^1−x1−E(x^1−x1))2=E(x^1−x1)2 需要解释为什么 E ( x ^ 1 − x 1 ) = 0 E(\hat x_1-x_1)=0 E(x^1−x1)=0，这是因为卡尔曼滤波是无偏估计，即状态最优估计与真实状态的期望相同。
以上推导将最优估计与 t r [ E ( x ^ n − x n ) ( x ^ n − x n ) T ] tr[E(\hat x_n-x_n)(\hat x_n-x_n)^T] tr[E(x^n−xn)(x^n−xn)T]挂钩。现在我们的任务就是最小化这个协方差矩阵的迹： arg min x ^ n t r [ C o v ( x ^ n − x n , x ^ n − x n ) ] \argmin_{\hat x_n} tr[Cov(\hat x_n-x_n, \hat x_n-x_n)] x^nargmintr[Cov(x^n−xn,x^n−xn)]
卡尔曼增益根据多维随机变量协方差的线性性质，简化 x ^ n − x n \hat x_n-x_n x^n−xn的协方差矩阵： C o v ( x ^ n − x n , x ^ n − x n ) = C o v ( x ^ n , x ^ n ) t r [ C o v ( x ^ n − x n , x ^ n − x n ) ] = t r [ C o v ( x ^ n , x ^ n ) ] Cov(\hat x_n-x_n,\hat x_n-x_n)=Cov(\hat x_n,\hat x_n) \\ tr[Cov(\hat x_n-x_n,\hat x_n-x_n)]=tr[Cov(\hat x_n,\hat x_n)] Cov(x^n−xn,x^n−xn)=Cov(x^n,x^n)tr[Cov(x^n−xn,x^n−xn)]=tr[Cov(x^n,x^n)] 至此，卡尔曼滤波中的状态最优估计问题，被转换为最小化最优估计的协方差矩阵的迹，而协方差矩阵的迹则通过卡尔曼增益来调整。 这就是各大博客资料里所谓的协方差矩阵均方误差最小化的本质。
接下来就要来解这个最小化问题。首先重新整理一下最优估计的公式： x ^ n = x ^ n ∣ n − 1 + K k Δ z ^ n = x ^ n ∣ n − 1 + K k z ^ n − K k H x ^ n ∣ n − 1 = ( I − K k H ) x ^ n ∣ n − 1 + K k H x n + K k v n \hat x_n = \hat x_{n|n-1}+K_k \Delta \hat z_n \\ = \hat x_{n|n-1}+K_k\hat z_n-K_kH \hat x_{n|n-1} \\ = (I-K_kH)\hat x_{n|n-1}+K_kHx_n + K_kv_n x^n=x^n∣n−1+KkΔz^n=x^n∣n−1+Kkz^n−KkHx^n∣n−1=(I−KkH)x^n∣n−1+KkHxn+Kkvn
t r [ C o v ( x ^ n , x ^ n ) ] = t r [ ( I − K k H ) P ^ n ∣ n − 1 ( I − K k H ) T + K k Q K k T ] = t r [ P n ∣ n − 1 − K k H P n ∣ n − 1 − P n ∣ n − 1 H T K k T + K k H P n ∣ n − 1 H T K k T + K k Q K k T ] tr[Cov(\hat x_n,\hat x_n)] = tr[(I-K_kH)\hat P_{n|n-1}(I-K_kH)^T + K_kQK_k^T] \\ = tr[P_{n|n-1}-K_kHP_{n|n-1}-P_{n|n-1}H^TK_k^T+K_kHP_{n|n-1}H^TK_k^T + K_kQK_k^T] \\ tr[Cov(x^n,x^n)]=tr[(I−KkH)P^n∣n−1(I−KkH)T+KkQKkT]=tr[Pn∣n−1−KkHPn∣n−1−Pn∣n−1HTKkT+KkHPn∣n−1HTKkT+KkQKkT]
现在就可以计算卡尔曼增益了： ∂ t r [ C o v ( x ^ n , x ^ n ) ] ∂ K k = − ( H P n ∣ n − 1 ) T − ( H P n ∣ n − 1 ) T + 2 K k ( H P n ∣ n − 1 H T + Q ) 2 ( H P n ∣ n − 1 ) T = 2 K k ( H P n ∣ n − 1 H T + Q ) K k = P n ∣ n − 1 T H T ( H P n ∣ n − 1 H T + Q ) − 1 \frac {\partial tr[Cov(\hat x_n,\hat x_n)]}{\partial K_k} = -(HP_{n|n-1})^T-(HP_{n|n-1})^T+ 2K_k(HP_{n|n-1}H^T+Q) \\ 2(HP_{n|n-1})^T=2K_k(HP_{n|n-1}H^T+Q) \\ K_k = P_{n|n-1}^TH^T(HP_{n|n-1}H^T+Q)^{-1} ∂Kk∂tr[Cov(x^n,x^n)]=−(HPn∣n−1)T−(HPn∣n−1)T+2Kk(HPn∣n−1HT+Q)2(HPn∣n−1)T=2Kk(HPn∣n−1HT+Q)Kk=Pn∣n−1THT(HPn∣n−1HT+Q)−1 获得卡尔曼增益 K k K_k Kk后，代入式 x ^ n = x ^ n ∣ n − 1 + K k Δ z ^ n \hat x_n = \hat x_{n|n-1}+K_k \Delta \hat z_n x^n=x^n∣n−1+KkΔz^n，就得到了状态最优估计，当前时刻的卡尔曼滤波就完成了。
后记以上就是卡尔曼滤波中，卡尔曼增益的推导了，着重在于最优估计与协方差矩阵的关系解读。
最后求协方差矩阵的迹的偏导时，需要矩阵求导相关的基础，过几天会在概率论专栏更新矩阵求导对应的内容。
推到中还提到了无偏估计，下一次将更新卡尔曼滤波的无偏估计性质以及证明。
最后，线性卡尔曼滤波即将结束，下下次会进入扩展卡尔曼滤波EKF的记录，并且后续会延伸到IKF、UKF、ESKF等相关算法。
