捷联惯导系统学习5.1(最小方差估计和线性最小方差估计)

最小方差估计(最小均方误差MMSE,也称条件期望估计)

使方差最小即：
$X:系统状态量$
$V:干扰噪声$
$\tilde{X}:估计值与系统状态误差$
$E_X:数学期望$
$$MMSE[\hat{X}]=MSE[\hat{X}{]}_{min}=tr(E[\tilde{X}{\tilde{X}}^T]{)}_{min}$$
M S E [ X ^ ] = E [ [ X − X ^ ] [ X − X ^ ] ] = ∫ − ∞ ∞ ∫ − ∞ ∞ [ x − X ^ ( z ) ] T [ x − X ^ ( z ) ] p ( x , z ) d x d z = ∫ − ∞ ∞ ∫ − ∞ ∞ [ x − X ^ ( z ) ] T [ x − X ^ ( z ) ] p ( x ∣ z ) p ( z ) d x d z = ∫ − ∞ ∞ P z ( z ) { x T x p ( x ∣ z ) d x − 2 X ^ ( z ) ∫ − ∞ ∞ x p ( x ∣ z ) d x + X ^ ( z ) T X ^ ( z ) ∫ − ∞ ∞ p ( x ∣ z ) d } d z = ∫ − ∞ ∞ P z ( z ) { E X [ X T X ∣ z ] − 2 X ^ ( z ) E X [ X ∣ z ] + X ^ ( z ) T X ^ ( z ) } d z = ∫ − ∞ ∞ P z ( z ) { E X [ X T X ∣ z ] − E X T [ X ∣ z ] E X [ X ∣ z ] E X T [ X ∣ z ] E X [ X ∣ z ] − 2 X ^ ( z ) E X [ X ∣ z ] + X ^ ( z ) T X ^ ( z ) } d z = ∫ − ∞ ∞ P z ( z ) { E X [ X T X ∣ z ] − E X T [ X ∣ z ] E X [ X ∣ z ] } d z + ∫ − ∞ ∞ P z ( z ) [ E X [ X ∣ z ] − X ^ ( z ) ] T [ E X [ X ∣ z ] − X ^ ( z ) ] d z MSE[\hat X]=E[[X-\hat X][X-\hat X]] \\ =\int_{-\infty}^{\infty}\int_{-\infty}^{\infty}[x-\hat X(z)]^T[x-\hat X(z)]p(x,z)dxdz\\ =\int_{-\infty}^{\infty}\int_{-\infty}^{\infty}[x-\hat X(z)]^T[x-\hat X(z)]p(x|z)p(z)dxdz\\ =\int_{-\infty}^{\infty}P_z(z)\{x^Txp(x|z)dx-2\hat X(z)\int_{-\infty}^{\infty} xp(x|z)dx+\hat X(z)^T\hat X(z)\int_{-\infty}^{\infty}p(x|z)d\}dz\\ =\int_{-\infty}^{\infty}P_z(z)\{E_X[X^TX|z]-2\hat X(z)E_X[X|z]+\hat X(z)^T\hat X(z)\}dz\\ =\int_{-\infty}^{\infty}P_z(z)\{E_X[X^TX|z]-E^T_X[X|z]E_X[X|z]E^T_X[X|z]E_X[X|z]-2\hat X(z)E_X[X|z]+\hat X(z)^T\hat X(z)\}dz \\ =\int_{-\infty}^{\infty}P_z(z)\{E_X[X^TX|z]-E^T_X[X|z]E_X[X|z]\}dz +\int_{-\infty}^{\infty}P_z(z)[E_X[X|z]-\hat X(z)]^T[E_X[X|z]-\hat X(z)]dz MSE[X^]=E[[X−X^][X−X^]]=∫−∞∞​∫−∞∞​[x−X^(z)]T[x−X^(z)]p(x,z)dxdz=∫−∞∞​∫−∞∞​[x−X^(z)]T[x−X^(z)]p(x∣z)p(z)dxdz=∫−∞∞​Pz​(z){xTxp(x∣z)dx−2X^(z)∫−∞∞​xp(x∣z)dx+X^(z)TX^(z)∫−∞∞​p(x∣z)d}dz=∫−∞∞​Pz​(z){EX​[XTX∣z]−2X^(z)EX​[X∣z]+X^(z)TX^(z)}dz=∫−∞∞​Pz​(z){EX​[XTX∣z]−EXT​[X∣z]EX​[X∣z]EXT​[X∣z]EX​[X∣z]−2X^(z)EX​[X∣z]+X^(z)TX^(z)}dz=∫−∞∞​Pz​(z){EX​[XTX∣z]−EXT​[X∣z]EX​[X∣z]}dz+∫−∞∞​Pz​(z)[EX​[X∣z]−X^(z)]T[EX​[X∣z]−X^(z)]dz
其中：
第一个式子 ∫ − ∞ ∞ P z ( z ) { E X [ X T X ∣ z ] − E X T [ X ∣ z ] E X [ X ∣ z ] } d z \int_{-\infty}^{\infty}P_z(z)\{E_X[X^TX|z]-E^T_X[X|z]E_X[X|z]\}dz ∫−∞∞​Pz​(z){EX​[XTX∣z]−EXT​[X∣z]EX​[X∣z]}dz与$\hat{X}(z)$无关;
$P_Z(Z)$为非负且不恒为0，$[E_X[X|z]-\hat{X}(z){]}^T[E_X[X|z]-\hat{X}(z)]$必定非负，想要协方差取最小值，必须满足：
$$E_X[X|z]-\hat{X}(z)=0$$
得到最小误差:${\hat{X}}_{MIN}(Z)=E[X|Z]$
得到最小方差求解：
$p_Z(z):边缘密度函数$
$p(x,z):条件概率密度函数$
$$\hat{X}(z)=E_X[X|Z]={\int }_{-\infty }^{\infty }xp(x|z)dx={\int }_{-\infty }^{\infty }x\frac{p(x,z)}{p_Z(z)}dx$$
得到最小方差求解的简化（假设估计量和观测量都符合正太分布）：
实际中，$p_Z(z):边缘密度函数$，$p(x,z):条件概率密度函数$很难得到，假设观测量与被测量都符合正太分布：
$C:协防差矩阵$
$m:均值$
$$\begin{gathered} Y\sim N(m_Y,C_Y) \\ Y=\left[\begin{array}{c}X\\ Z\end{array}\right],m_Y=\left[\begin{array}{c}{m}_X\\ m_Z\end{array}\right],C_Y=\left[\begin{array}{cc}{C}_X& C_{XZ}\\ C_{ZX}& C_Z\end{array}\right] \\ {C}_{XZ}=Cov(X,Z)=Co{v}^T(Z,X) \end{gathered}$$
Y的正太分布密度函数为：
$|C_Y{|}^{=}det(C_Y)$
$$p(y)=p(x,z)=\frac{1}{(2\pi {)}^{(n+m)/2}|C_Y{|}^{1/2}}{e}^{-\frac{1}{2}(y-m_Y{)}^T{C}_Y^{-1}(y-m_Y)}$$
已知：
$m_{X|Z}=m_X+C_{XZ}{C}_Z^{-1}(z-m_Z)$
$C_{X|Z}=C_X-C_{XZ}{C}_Z^{-1}{C}_{ZX}$
可以得到（推导略）：
$$\begin{gathered} p(x,z)=\frac{1}{(2\pi {)}^{n/2}|C_{X|Z}{|}^{1/2}}{e}^{-\frac{1}{2}(x-m_{X|Z}{)}^T{C}_{X|Z}^{-1}(x-m_{X|Z})}\times p_Z(z) \\ {p}_Z(z)={\int }_{-\infty }^{\infty }p(x,z)dx=\frac{1}{(2\pi {)}^{m/2}|C_{X|Z}{|}^{1/2}}{e}^{-\frac{1}{2}(x-m_Z{)}^T{C}_Z^{-1}(x-m_Z)} \\ p(x|z)=\frac{1}{(2\pi {)}^{n/2}|C_{X|Z}{|}^{1/2}}{e}^{-\frac{1}{2}(x-m_{X|Z}{)}^T{C}_{X|Z}^{-1}(x-m_{X|Z})} \end{gathered}$$
得到正太分布的最小均值和方差矩阵为：
$$最小均值：{\hat{X}}_{MV}={\hat{X}}_{MIN}(Z)=E[X|Z]={\int }_{-\infty }^{\infty }xp(x|z)dz=m_{X|Z}=m_X+C_{XZ}{C}_Z^{-1}(z-m_z)$$
$$方差：E[{\tilde{X}}_{MV}{\tilde{X}}_{MV}^T]={\int }_{-\infty }^{\infty }{C}_{X|Z}{P}_z(z)dz=C_X-C_{XZ}{C}_Z^{-1}{C}_{ZX}$$

最小均方误差观测量与系统状态量的关系：
$m_{X|Z}=m_X+C_{XZ}{C}_Z^{-1}(z-m_Z)$
$C_{X|Z}=C_X-C_{XZ}{C}_Z^{-1}{C}_{ZX}$
当$C_{X|Z}->0(或C_Z->\infty )$,$m_{X|Z}->m_X$,此时观测量与状态量相关性很小，m_X越等于最好估计。
$C_{X|Z}=C_X-C_{XZ}{C}_Z^{-1}{C}_{ZX}<C_X$引入观测后，不确定比$C_X(先验误差)$降低了

线性最小方差估计（LMV）

精度不如MMSE，但计算量小,对于正太分布相性最小方差等价与最小均方误差估计。
无论观测模型是线性的还是非线性的，均使用观测量的线性组合建模来对状态进行估计。
指标函数为：
$$LMV(X)=E[{\tilde{X}}^T\tilde{X}]{|}_{\hat{x}={\hat{x}}_{LMV}}$$
线性模型：$$\hat{X}=AZ+b$$
做变换$$\begin{gathered} LMV(X)=tr(E[{\tilde{X}}^T\tilde{X}])=tr(E[(X-AZ-b)(X-AZ-b))]) \\ =tr((A-C_{XZ}{C}_Z^{-1})C_Z(A-C_{XZ}{C}_Z^{-1}{)}^T)+tr(C_X-C_{XZ}{C}_Z^{-1}{C}_{ZX})+tr((m_X-A{m}_Z-b)(m_X-A{m}_Z-b)) \end{gathered}$$
可知：
$(A-C_{XZ}{C}_Z^{-1})C_Z(A-C_{XZ}{C}_Z^{-1}{)}^T$:非负
$tr(C_X-C_{XZ}{C}_Z^{-1}{C}_{ZX})$:与待定系数无关
$tr((m_X-A{m}_Z-b)(m_X-A{m}_Z-b))$:非负
想求的最小值只需
$$\{\begin{array}{ll}{m}_X-A{m}_Z-b=0& \\ A-C_{XZ}{C}_Z^{-1}=0& \end{array}$$
可解得：$A=C_{XZ}{C}_Z^{-1},b=m_X-C_{XZ}{C}_Z^{-1}{m}_Z$
得到最小方差的指标函数：
$$LMV(\hat{X})=C_{XZ}{C}_Z^{-1}Z+m_X-C_{XZ}{C}_Z^{-1}{m}_Z$$
得到线性最小方差的期望：
$$E_Z[{\hat{X}}_{LMV}]=E(X)$$
得到线性最小方差的协方差：
$$E[{\tilde{X}}_{LMV}{\tilde{X}}_{LMV}^T]=C_X-C_{XZ}{C}_Z^{-1}{C}_{ZX}$$
观测量与系统状态量的关系：
$$Cov({\tilde{X}}_{LMV},Z)=0$$
这表明线性最小方差的估计量与观测量Z不相关