自适应卡尔曼滤波的噪声方差怎么推的_卡尔曼滤波的推导

卡尔曼滤波的推导

1 最小二乘法

在一个线性系统中，若\(x\)为常量，是我们要估计的量，关于\(x\)的观测方程如下：

\[y = Hx + v \tag{1.1}

\]

\(H\)是观测矩阵(或者说算符)，\(v\)是噪音，\(y\)是观察量。若关于\(x\)的最佳估计为\(\hat{x}\),误差可定义为\(\epsilon_y\):

\[\epsilon_y = y - H\hat{x} \tag{1.2}

\]

定义代价函数\(J\)为：

\[J = \epsilon_{y1}^2 + \epsilon_{y2}^2 + ... + \epsilon_{yk}^2 \tag{1.3}

\]

将方程\((1.2)\)代入\((1.3)\)可得：

\[J=\left(y-H\hat{x}\right)^T\left(y-H\hat{x}\right)=y^Ty-x^TH^Ty-y^TH\hat{x}+\hat{x}^TH^TH\hat{x} \tag{1.4}

\]

使代价函数\(J\)最小的就是最佳估计，对\(J\)求偏导，并使之为0：

\[\frac{\partial J}{\partial \hat{x}}=-2y^TH+2\hat{x}^TH^TH=0 \tag{1.5}

\]

解得：

\[\hat{x} = \left(H^TH\right)^{-1}H^Ty \tag{1.6}

\]

2 递推最小二乘法

线性系统的递推估计值可以写成以下形式：

\[y_k=H_kx+v_k \tag{2.1}

\]

\[\hat{x}_k = \hat{x}_{k-1} + K_k\left(y_k-H_k\hat{x}_{k-1}\right) \tag{2.2}

\]

其中是\(K_k\)待定的增益矩阵，\(y_k-H_k\hat{x}_{k-1}\)为修正项。先考虑线性递推估计器的估计误差均值,要注意的是这里和式\((1.2)\)有所不同，但本质都是一样东西：

\[\begin{split}

E\left(\epsilon_{x,k}\right) &= E\left(x-\hat{x}_k\right) \cr

&= E\left(x- \hat{x}_{k-1}-K_k\left(y_k-H_k\hat{x}_{k-1}\right)\right) \newline

&= E\left(\epsilon_{x,k-1}-K_k\left(H_kx+v_k-H_k\hat{x}_{k-1}\right)\right) \cr

&= E\left(\epsilon_{x,k-1}-K_kH_k\left(x-\hat{x}_{k-1}\right)-K_kv_k\right) \cr

&=\left(I-K_kH_k\right)E\left(\epsilon_{x,k-1}\right)-K_kE\left(v_k\right)

\end{split} \tag{2.3}

\]

类似地，代价函数\(J_k\)表达为：

\[J_k=E(\epsilon_{x1,k}^2+\epsilon_{x2,k}^2+...+\epsilon_{xn,k}^2)=E(\epsilon_{x,k}^T\epsilon_{x,k})=E(Tr(\epsilon_{x,k}\epsilon_{x,k}^T))=Tr(P_k) \tag{2.4}

\]

其中\(P_k\)为估计误差的协方差矩阵，利用式\((2.3)\)可以得到：

\[\begin{split}

P_k &= E\left(\epsilon_{x,k}\epsilon_{x,k}^T\right) \newline

&=E\left\{\left[(I-K_kH_k)E(\epsilon_{x,k-1})-K_kE(v_k)\right]\left[(I-K_kH_k)E(\epsilon_{x,k-1})-K_kE(v_k)\right]^T\right\} \cr

&=(I-K_kH_k)E(\epsilon_{x,k}\epsilon_{x,k}^T)(I-K_kH_k)^T - K_kE(v_k\epsilon_{x,k-1}^T) \cr

& \quad -(I-K_kH_k)E(\epsilon_{x,k-1}v_k^T)K_k^T+K_kE(v_kv_k^T)K_k^T

\end{split} \tag{2.5}

\]

由于\(\epsilon_{x,k-1}\)与\(v_k\)是相互独立的，所以有：