假设有线性离散系统如下：
\[\begin{align}
x_k&=F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+\omega_{k-1} \tag{1} \\
y_k&=H_kx_k+\nu_k \tag{2}
\end{align}
\]
\( \{\omega_k\}\)和\( \{\nu_k\} \)是零均值，不相关的白噪声，有已知的协方差矩阵\( Q_k\)和\( R_k\),既：
\[\begin{align}
\omega_k\thicksim(0,Q_k)\\
\nu_k\thicksim(0,R_k)\\
E[\omega_k\omega_j^T]=Q_k\delta_{k-j}\\
E[\nu_k\nu_j^T]=R_k\delta_{k-j}\\
E[\nu_k\omega_j^T]=0
\end{align}
\]
其中\( \delta_{k-j}\)是\( Kronecker-\delta\)函数，既如果\( k=j\)那么\( \delta_{k-j}=1\)，如果\( k\neq j\)那么\( \delta_{k-j}=0\)。我们的目的是在已知的系统动态方程和带噪声量测\( \{ y_k\}\)的基础上估计状态量\( \{x_k\}\)。对于状态估计可用的信息量，取决于我们要解决的问题本身。
如果我们利用包括\( k\)时刻和\( k\)时刻以前的量测值估计\( x_k\)，那么能得到一个后验估计，表示为\( \hat{x}_k^+\)。上标“+”表示这个估计是后验的。获得后验状态估计的方法是在\( k\)时刻和\( k\)时刻以前的量测值的条件下计算\( x_k\)的期望值，即
\[\hat{x}_k^+ = E[x_k|y_1,y_2,\cdots,y_k]=后验估计
\]
如果我们利用\( k\)时刻之前（不包括\( k\)）的量测值来估计\( x_k\)，那么能得到一个先验估计，表示为\( \hat{x}_k^-\)。上标“-”表示这个估计是先验的。获得先验状态估计的方法是在时间\( k\)时刻以前的量测值的条件下计算\( x_k\)的期望值，即
\[\hat{x}_k^+ = E[x_k|y_1,y_2,\cdots,y_{k-1}]=先验估计
\]
注意，\( \hat{x}_k^+\)和\( \hat{x}_k^-\)都是同一个量的估计，都是\( x_k\)的估计值。然而\( \hat{x}_k^-\)是在考虑量测值\( y_k\)之前对\( x_k\)的估计，\( \hat{x}_k^+\)是在考虑量测值\( y_k\)之后对\( x_k\)的估计。 因为我们用更多的信息计算\( \hat{x}_k^+\)，所以自然希望\( \hat{x}_k^+\)是比\( \hat{x}_k^-\)更好的估计值：
\[\hat{x}_k^-＝获得k时刻量测值之前在时间点k的x_k的估计\\
\hat{x}_k^+＝获得k时刻量测值之后在时间点k的x_k的估计
\]
我们用\( \hat{x}_0^+\)表示未使用任何量测值前\( x_0\)的初始估计。第一个量测值是在时间\( k=1\)时计算的。由于我们没有任何可用的量测值来估计\( x_0\)，所以\( \hat{x}_0^+\)作为初始状态\( x_0\)的期望值是合理的，即
\[\hat{x}_0^+=E[x_0]
\]
我们用\( P_k\)表示估计误差的协方差。\( P_k^-\)表示\( \hat{x}_k^-\)的估计误差协方差，\( P_k^+\)表示\( \hat{x}_k^+\)的估计误差协方差：
\[P_k^-=E[(x_k-\hat{x}_k^-)(x_k-\hat{x}_k^-)^T]\\
P_k^+=E[(x_k-\hat{x}_k^+)(x_k-\hat{x}_k^+)^T]
\]
以\( x_0\)的估计协方差\( P_0^+\)开始。如果完全地了解初始状态那么\( P_0^+=0\)。如果对于\( x_0\)的值没有信息，那么\( P_0^+=\infty I\)。通常，\( P_0^+\)代表\( x_0\)初始估计值的不确定性。
\[P_0^+=E[(x_0-\hat{x}_0^+)(x_0-\hat{x}_0^+)^T]
\]
均值和协方差的时间更新
如上所提，线性离散系统如下：
\[x_k=F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+\omega_{k-1} \tag{3}
\]
先推导状态\(x_k\)的均值将会随时间有怎样的变化？如果对式(3)两边取期望将会得到
\[\bar{x}_k =E[x_k] = F_{k-1} \bar{x}_ {k-1} + G_{k-1}u_ {k-1} \tag{4}
\]
再推导状态\( x_k\)的协方差将会随时间有怎样的变化？通过式(3)和式(4)得到
\[\begin{align}
&(x_k-\bar{x}_k)(x_k-\bar{x}_k)^T\\
=&(F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+\omega_{k-1}-\bar{x}_ k)(F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+\omega_{k-1}-\bar{x}_ k)^T\\
=&[F_ {k-1}(x_ {k-1}-\bar{x}_ {k-1})+\omega_ {k-1}] [F_ {k-1}(x_ {k-1}-\bar{x}_ {k-1})+\omega_ {k-1}]^T\\
=&F_{k-1}(x_{k-1}-\bar{x}_ {k-1})(x_{k-1}-\bar{x}_ {k-1})^TF_{k-1}^T +\omega_{k-1}\omega_{k-1}^T+\\
&F_{k-1}(x_{k-1}-\bar{x}_ {k-1})\omega_{k-1}^T+\omega_{k-1}(x_{k-1}-\bar{x}_ {k-1})^TF_{k-1}^T
\end{align}
\]
上式的期望值即为\(x_k\)的协方差。 由于\((x_{k-1}- \bar{x} _ {k-1})\)与\(\omega _ {k-1}\)不相关，因此
\[P_k=E[(x_k-\bar{x})(x_k-\bar{x})^T]=F_{k-1}P_{k-1}F_{k-1}^T+Q_{k-1}
\]
因此我们可以通过前一时刻的后验估计，获得本时刻的先验估计：
\[\hat{x}_k^-=F_{k-1}\hat{x}_ {k-1}^+ + G_{k-1}u_{k-1} \tag{5}
\]
再通过前一时刻的后验估计协方差，获得本时刻的先验估计协方差：
\[P_k^- = F_{k-1} P_{k-1}^+ F_{k-1}^T+Q_{k-1} \tag{6}
\]
均值和协方差的量测更新
假设我们在获取先验估计值之后，获取了一个新的测量值\( y_k\)，那么我们如何计算更新估计值呢？线性的递推估计值可以写成如下形式：
\[\begin{align}
y_k&=H_kx_k+\nu_k \tag{7}\\
\hat{x}_k^+&=\hat{x}_k^-+K_k(y_k-H_k\hat{x}_k^-) \tag{8}
\end{align}
\]
也就是说，我们基于先验估计值\( \hat{x}_k^-\)和新获得的测量值\( y_k\)来计算后验估计值\( \hat{x}_k^{+} \)，其中\( K_k\)是待定增益矩阵。而\( (y_k-H_k\hat{x}_k^-)\)被称为修正项。 如果修正项为零，或者增益矩阵为零，那么估计值的大小在由\( k-1\)步到\( k\)步则不会改变。
在我们计算最佳增益矩阵\( K_k\)之前，让我们考虑一下线性递推估计器的估计误差均值。估计误差均值可以表示为
\[\begin{align}
E&[\varepsilon_{x,k}^+]\\
=&E(x_k-\hat{x}_k^+)\\
=&E(x_k-\hat{x}_k^- – K_k(y_k-H_k\hat{x}_k^-))\\
=&E(\varepsilon _ {x,k}^- – K_k(H_kx_k+\nu_k-H_k\hat{x}_k^-))\\
=&E(\varepsilon _ {x,k}^- – K_kH_k(x_k-\hat{x}_k^-)-K_k\nu_k)\\
=&(I-K_kH_k)E[\varepsilon_{x,k}^-]-K_kE[\nu_k]
\end{align}
\]
所以，如果\( E[\varepsilon_{x,k}^-]=0\)并且\( E[\nu_k]=0\)，那么\( E[\varepsilon_{x,k}^+]=0\)。换句话说，如果测量值噪声\( \nu_k\)是零均值的，并且\( x_k\)的先验估计\( \hat{x}_k^-\)等于\( x_k\)的期望值, 那么\( x_k\)的后验估计\( \hat{x}_k^+\)对于所有的\( K_k\)而言都等于\( x_k\)的期望值。因此，估计被称为无偏估计。注意，这个特性与增益矩阵\( K_k\)的值无关。这对千估计器来说是一个非常好的特性，因为它说明估计值的平均值将会等于真实值。
下面我们把注意力转移到求取\(K_k\)的最佳值上。因为估计器的无偏性与我们使用的\( K_k\)取值无关，所以我们需要选择其他标准来决定\(K_k\)的取值。 我们选择的最优标准是使\(k\)时刻估计误差的方差和最小。
\[\begin{align}
J_k=&E[(x_1-\hat{x}_1)^2]+\cdots+E[(x_n-\hat{x}_n)^2]\\
=&E[\varepsilon_{x_1,k}^2]+\cdots+E[\varepsilon_{x_n,k}^2]\\
=&E\{Tr[(\varepsilon_{x,k}^+)(\varepsilon_{x,k}^+)^T]\}\\
=&TrP_k^+
\end{align}
\]
其中\( P_k^+\)为估计误差的协方差矩阵，在上式中被定义。我们可以递推来计算\(P_k^+\)，即
\[\begin{align}
P_k^+ = &E[(\varepsilon_{x,k}^+)(\varepsilon_{x,k}^+)^T]\\
= &E\{[(I-K_kH_k)(\varepsilon_{x,k}^-)-K_k\nu_k][(I-K_kH_k)(\varepsilon_{x,k}^-)-K_k\nu_k]^T\}\\
=& (I-K_kH_k)E[(\varepsilon_{x,k}^-)(\varepsilon_{x,k}^-)^T] (I-K_kH_k)^T -\\
&K_kE[\nu_k(\varepsilon_{x,k}^-)^T] (I-K_kH_k)^T-(I-K_kH_k)E[(\varepsilon_{x,k}^-)\nu_k^T]K_k^T+\\
&K_kE(\nu_k\nu_k^T)K_k^T
\end{align}
\]
注意，\(\varepsilon _ {x,k} ^ – \)(\(k\)时刻的估计误差）独立于\(\nu_k\)(\(k\)时刻的测量噪声）。因此
\[E[ \nu _k (\varepsilon_{x,k}^-) ^ T ] = E( \nu _ k )E( \varepsilon _ {x,k}^-)=0
\]
上式中所有的期望值都为零。因此有
\[P_k^+=(I-K_kH_k)P_{k}^-(I-K_kH_k)^T+K_kR_kK_k^T \tag{9}
\]
其中\(R_k\)是\( \nu_k\)的协方差。这就是最小二乘估计误差的协方差递推计算公式 。这和我们的直觉是一致的：如果测量噪声增加了（\( R_k\)增加），那么估计值的不确定性也会增加（\( P_k^+\)增加）。注意，因为\( P_k^+\)是一个协方差矩阵，所以它是正定的，并且假设\( P_{k}^-\)和\( R_k\)是正定的，则式保证了\( P_k^+\)是正定的。
现在我们需要找出一个\( K_k\)的值使式中的代价函数尽可能的小。 对于任何\( K_k\)来说，估计误差的均值都是零。所以如果我们选择\( K_k\)使代价函数（\( P_k^+\)的迹）最小，那么估计误差不仅为零均值，并且会一直趋于零。我们对\( K_k\)求导获得：
\[\frac{ \partial J_k }{\partial K_k }=2(I-K_kH_k)P_{k}^-(-H_k^T)+2K_kR_k
\]
为了找出一个使\( J_k\)最小的\( K_k\)，我们使上述微分为零，则可以解出\( K_k\)如下：
\[K_k=P_{k}^-H_k^T(H_kP_k^-H_k^T+R_k)^{-1} \tag{10}
\]
式(8)式(9)和式(10)构成了基于最小均方误差准则的量测更新。
标准卡尔曼滤波总结
1.动态系统方程如下：
\[x_k=F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+\omega_{k-1}\\
y_k=H_kx_k+\nu_k
\] \[
\omega_k\thicksim(0,Q_k)\\
\nu_k\thicksim(0,R_k)\\
E[\omega_k\omega_j^T]=Q_k\delta_{k-j}\\
E[\nu_k\nu_j^T]=R_k\delta_{k-j}\\
E[\nu_k\omega_j^T]=0
\]
2.卡尔曼滤波初始化条件如下：
\[\hat{x}_0^+=E[x_0]\\
P_0^+=E[(x_0-\hat{x}_0^+)(x_0-\hat{x}_0^+)^T]
\]
3.卡尔曼滤波每一步计算如下，其中\( k=1,2,\cdots\)
\[\begin{align}
\hat{x}_k^-&=F_{k-1}\hat{x} _ {k-1}^++G_{k-1}u_{k-1} \tag{11}\\
P_k^- &= F_{k-1} P_{k-1}^+ F_{k-1}^T+Q_{k-1} \tag{12}\\
K_k&=P_k^-H_k^T(H_kP_k^-H_k^T+R_k)^{-1}=P_k^+H_k^TR_k^{-1} \tag{13}\\
\hat{x}_k^+&=\hat{x}_k^-+K_k(y_k-H_k\hat{x}_k^-) \tag{14}\\
P_k^+ &=(I-K_kH_k)P_k^-(I-K_kH_k)^T+K_kR_kK_k^T\\
&=[(P_k^-)^{-1}+H_k^TR_k^{-1}H_k]^{-1}\\
&=(I-K_kH_k)P_k^-
\end{align}
\]