Kalman filter 推导

最新推荐文章于 2024-03-26 00:07:31 发布

原创最新推荐文章于 2024-03-26 00:07:31 发布 · 291 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#Kalman filter 卡尔曼滤波器

一些基本数学知识

均方误差
$MSE=E(x^−x)2MSE = E(\hat{x} - x)^2$
协方差矩阵
$\mu)(x - \mu)^T] = \Sigma$
其实若估计值是无偏估计时，即 $μe=0\mu_e = 0$ 时， $t r (C)$ 就是MSE。
线性系统状态差分方程 $x_k = Ax_{k-1} +Bu_{k-1}+ w_{k-1}$ 系统观测方程 $z_k = Hx_k +v_k$

推导过程

注意区分估计值 $xk^\hat{x_k}$ 和预测值 $xk~\tilde{x_k}$
$xk^=xk~+Kk(zk−Hxk)\hat{x_k} = \tilde{x_k} + K_k(z_k - Hx_k)$ 为使每时刻的估计值都为最优估计，因此需要寻找一个目标函数，来求取每时刻使目标函数最小的增益矩阵 $K_k$
使用真实值与估计值之间的误差的协方差阵作为目标函数，即 $Pk=E[(ek)(ek)T]=E[(x−xk^)(x−xk^)T]P_k = E[(e_k)(e_k)^T] = E[(x - \hat{x_k})(x - \hat{x_k})^T]$
$K_kH)(x_k - \tilde{x_k}) - k_kv_k) *((I - K_kH)(x_k - \tilde{x_k}) - k_kv_k)^T]$
由于测量噪声为白噪声， $E(v_k) = 0$ ，因此 $P_k$ 可简化为
$Pk=(I−KkH)E[(xk−xk~)(xk−xk~)T](I−KkH)T+KkE(vkvkT)KkTP_k = (I - K_kH)E[(x_k - \tilde{x_k})(x_k - \tilde{x_k})^T] (I - K_kH)^T + K _kE(v_kv_k^T)K_k^T$
令 $E[(xk−xk~)(xk−xk~)T]=Pk~E[(x_k - \tilde{x_k})(x_k - \tilde{x_k})^T] = \tilde{P_k}$ , $E(v_kv_k^T) = R$

则 $Pk=(I−KkH)Pk~(I−KkH)T+KkRKkTP_k = (I - K_kH) \tilde{P_k}(I - K_kH)^T + K _kRK_k^T$
$\tilde{P_k} - K_kH\tilde{P_k} - \tilde{P_k}H^TK_k^T + K_k(H\tilde{P_k}H^T + R)K_k^T$
求改协方差矩阵的最小值与求改协方差矩阵的迹的最小值等价，因此
$tr(Pk）=tr(Pk~)−2tr(KkHPk~)−tr(Kk(HPk~HT+R)KkT)tr(P_k）= tr(\tilde{P_k}) - 2tr(K_kH\tilde{P_k} ) - tr(K_k(H\tilde{P_k}H^T + R)K_k^T)$
$∂tr(PK)∂Kk=−2(HPk~)T−2Kk(HPk~HT+R)=0\frac{\partial tr(P_K)}{\partial K_k } = -2(H\tilde{P_k})^T - 2K_k(H\tilde{P_k}H^T + R ) = 0$
$K_k = \tilde{P_k^T}H^T(H\tilde{P_k}H^T + R )^{-1}$
将 $K_k$ 带入 $P_k$ 中得到
$Pk=(I−KkH)Pk~P_k = (I - K_kH)\tilde{P_k}$
$Pk~=E[(x−xk~)(x−xk~)T]\tilde{P_k} = E[(x - \tilde{x_k})(x - \tilde{x_k})^T]$
$=E[((Axk−1+wk)−Ax^k−1)((Axk−1+wk)−Ax^k−1)T] = E[((Ax_{k-1} +w_k) - A\hat{x}_{k-1})((Ax_{k-1} +w_k) - A\hat{x}_{k-1})^T]$
$=E[(A(xk−1−x^k−1)(xk−1−x^k−1)TAT]+E(wk−1wk−1T)=E[(A(x_{k-1} - \hat{x}_{k-1})(x_{k-1} - \hat{x}_{k-1})^TA^T] +E(w_{k-1}w_{k-1}^T)$
$AP_{k-1}A^T +Q$
由此得到 $P~k\tilde P_k$ 的递推公式

算法过程

初始化k时刻的协方差矩阵 $P_k$ 和k时刻的估计值 $x^k\hat{x}_k$
计算K+1时刻的 $P~k+1\tilde P_{k+1}$ 和K+1时刻的预测值 $x~k+1\tilde x_{k+1}$
$P~k+1=AP^kAT+Q; x~k+1=Axk^+Buk \tilde P_{k+1} =A\hat P_kA^T+Q; \tilde x_{k+1}=A\hat{x_k}+Bu_k$
计算K+1时刻的增益矩阵 $K_{k+1}$ $Kk+1=P~k+1HT(HP~k+1HT+R)−1K_{k+1} = \tilde P_{k+1}H^T(H\tilde P_{k+1}H^T+R)^{-1}$
计算K+1时刻的估计值 $x^k+1和K+1时刻协方差矩阵Pk+1\hat x_{k+1}和K+1时刻协方差矩阵P_{k+1}$ $x^k+1=x~k+1+Kk+1(zk−Hx~k+1)\hat x_{k+1} = \tilde x_{k+1}+K_{k+1}(z_k-H\tilde x_{k+1})$
$Pk+1=(I−KK+1H)P~k+1P_{k+1} = (I-K_{K+1}H)\tilde P_{k+1}$