捷联惯导系统学习5.3(kalman 滤波方程推导 )

什么是状态空间

什么是状态空间
状态是指在系统中可决定系统状态、最小数目变量的有序集合。 而所谓状态空间则是指该系统全部可能状态的集合。
状态空间表示法
即为一种将物理系统表示为一组输入、输出及状态的数学模式，而输入、输出及状态之间的关系可用许多一阶微分方程来描述。
为了使数学模式不受输入、输出及状态的个数所影响，输入、输出及状态都会以向量的形式表示，而微分方程（若是线性非时变系统，可将微分方程转变为代数方程）则会以矩阵的形式来来表示。
状态转移矩阵 （转移概率矩阵）
状态转移是指客观事物由一种状态转移到另一种状态的概率
其他

随机系统状态空间模型

系统状态空间
$X_k:n维状态向量$
$Z_k:m维测量向量$
${\Phi }_{k/k-1}:已知的系统结构参数$
${\Gamma }_{k/k-1}:已知的系统结构参数，分别为n\times l阶系统分配噪声$
$H_k:已知的系统结构参数，分别为m\times n阶测量矩阵$
$V_k:m维测量噪声，高斯白噪声，服从正太分布$
$W_{k-1}:m维系统噪声向量，高斯白噪声，服从正太分布$
$V_k与W_{k-1}互不相关$
$$\begin{gathered} \{\begin{array}{l}{X}_k={\Phi }_{k/k-1}{X}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array} \\ st. \\ \{\begin{array}{ll}E[W_k]=0,E[W_k{W}_j^T]=Q_k{\delta }_{kj}& Q_k\ge 0\\ E[V_k]=0,E[V_k{V}_j^T]=R_k{\delta }_{kj},E[W_k{V}_j^T]=0& R\ge 0\end{array} \end{gathered}$$

滤波方程的推导

k-1时刻系统的观测方程和系统方程
使用最小方差估计：
${\hat{X}}_{k-1}:状态估计值$
${\tilde{X}}_{k-1}:状态估计值与实际值的误差$
$P_{k-1}:均方误差$
$$\begin{gathered} {\tilde{X}}_{k-1}=X_{k-1}-{\hat{X}}_{k-1}（系统方程） \\ {P}_{k-1}=E[{\tilde{X}}_{k-1}{\tilde{X}}_{k-1}^T]=E[(X_{k-1}-{\hat{X}}_{k-1})(X_{k-1}-{\hat{X}}_{k-1}{)}^T]（观测方程） \end{gathered}$$
根据状态估计值（${\hat{X}}_{k-1}$） 获得系统方程最优值(0均值噪声不会对最优值造成影响)
$X_{k/k-1}:最优估计值(也称状态一步估计)$
$${\hat{X}}_{k/k-1}=E[{\Phi }_{k/k-1}{X}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}]={\Phi }_{k/k-1}{X}_{k-1}$$
获得系统方程最优估计值测量误差
${\tilde{X}}_{k/k-1}:获得最优估计值测量误差$
$${\tilde{X}}_{k/k-1}={\Phi }_{k/k-1}{X}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}-{\hat{X}}_{k/k-1}={\Phi }_{k/k-1}{\tilde{X}}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}$$
获得系统方程最优估计值的均方误差矩阵
${\tilde{X}}_{k-1}与W_{k-1}不相关$
$P_{k/k-1}:最优估计值的均方误差$
$$\begin{gathered} P_{k/k-1}=E[{\tilde{X}}_{k-1}{\tilde{X}}_{k-1}^T] \\ =E[{\tilde{X}}_{k-1}{\tilde{X}}_{k-1}^T] \\ =E[({\Phi }_{k/k-1}{\tilde{X}}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1})({\Phi }_{k/k-1}{\tilde{X}}_{k-1}+{\Gamma }_{k/k-1}{W}_{k-1}{)}^T] \\ ={\Phi }_{k/k-1}E[{\tilde{X}}_{k-1}{\tilde{X}}_{k-1}^T]{\Phi }_{k/k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T \\ ={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1} \end{gathered}$$
同理可得测量方程的，估计值、误差、均方差矩阵
$$\{\begin{array}{ll}{\hat{Z}}_{k/k-1}=E[H_k{X}_k+V_k]=H_k{\hat{X}}_{k/k-1}& 最优估计值/最优一步估计\\ {\tilde{Z}}_{k/k-1}=Z_k-{\hat{Z}}_{k/k-1}=(H_k{X}_k+V_k)-H_k{\hat{X}}_{k/k-1}=H_k{\tilde{X}}_{k/k-1}+V_k& 最优估计值的误差/最优一步估计误差\\ P_{ZZ,k/k-1}=E[{\tilde{Z}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T]=H_{k}E[{\tilde{X}}_{k/k-1}{\tilde{X}}_{k/k-1}^T]H_k^T+E[V_k{V}_k^T]=H_k{P}_{k/k-1}{H}_k^T+R_k& 最优一步估计的均方差\\ P_{XZ,k/k-1}=E[{\tilde{X}}_{k/k-1}{\tilde{Z}}_{k/k-1}^T]=E[{\tilde{X}}_{k/k-1}(H_k{\tilde{X}}_{k/k-1}+V_k{)}^T]=P_{k/k-1}{H}_k^T& 最优一步估计的协方差\end{array}$$
修正系数$K_k$
从上方的计算可以的得到，测量方程和系统方程的最优值，使用${\hat{Z}}_{k/k-1}（测量方程一步估计误差）$修正${\hat{X}}_{k/k-1}（系统方程一步估计值）$可以进一步提高精度，即：
$K_k:误差修正系数（滤波增益filtergain）$
${\tilde{Z}}_{k/k-1}\widehat{:}新息（innovation）$
$$\begin{gathered} {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k{\tilde{Z}}_{k/k-1}(当前值=前一状态估计值+K_k测量预测误差) \\ =(I-K_k{H}_k){\hat{X}}_{k/k-1}+K_k{Z}_k \\ =(I-K_k{H}_k){\Phi }_{k/k-1}{\hat{X}}_{k-1}+K_k{Z}_k \end{gathered}$$

修正系数$K_k$的确定

k时刻系统的观测方程和系统方程
$${\tilde{X}}_k=X_k-{\hat{X}}_k$$
${\hat{X}}_k=(I-K_k{H}_k){\hat{X}}_{k/k-1}+K_k{Z}_k带入$
$Z_k=H_k{X}_k+V_k$
$${\tilde{X}}_k=(I-K_k{H}_k){\tilde{X}}_{x/x-1}-K_k{V}_k$$
k时刻${\hat{X}}_k$的均方误差阵
$$\begin{gathered} P_k=E[{\tilde{X}}_k{\tilde{X}}_k^T] \\ =E[(I-K_k{H}_k){\tilde{X}}_{x/x-1}-K_k{V}_k)(I-K_k{H}_k){\tilde{X}}_{x/x-1}-K_k{V}_k{)}^T] \\ =(I-K_k{H}_k)E[X_{x/x-1}{X}_{x/x-1}^T](I-K_k{H}_k{)}^T+K_{k}E[V_k{V}_k^T]K_k^T \\ =(I-K_k{H}_k)P_{k/k-1}(I-K_k{H}_k{)}^T+K_k{R}_k{K}_k^T \end{gathered}$$
k时刻估计误差最小
$$\begin{gathered} E[{\tilde{X}}_k{\tilde{X}}_k^T]{|}_{min}=tr(P_k{)}_{min} \\ =tr((I-K_k{H}_k)P_{k/k-1}(I-K_k{H}_k{)}^T+K_k{R}_k{K}_k^T) \\ =tr(P_{k/k-1})-tr(K_k{H}_k{P}_{k/k-1})-tr((K_k{H}_k{P}_{k/k-1}{)}^T)+tr(K_k(H_k{P}_{k/k-1}{H}_k^T+R_k)K_k^T) \end{gathered}$$
k时刻估计误差求极小值，即导数值为0的点
已知两个方阵迹的求导等式：
$\frac{d}{dX}tr(XB)=\frac{d}{dX}tr((XB{)}^T)=B^T$
$\frac{d}{dX}tr(XA{X}^T)=2XA$
$$\begin{gathered} \frac{d}{d{K}_k}tr(P)=0-(H_k{P}_{k/k-1}{)}^T-(H_k{P}_{k/k-1}{)}^T+2K_k(H_k{P}_{k/k-1}{H}_k^T+R_k) \\ =2[K_k(H_k{P}_{k/k-1}{H}_k^T+R_k)-P_{k/k-1}{H}_k^T]=0 \end{gathered}$$
解得$K_k$
$$K_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T-R_k{)}^{-1}$$
$$P_k=(I-K_k{H}_k)P_{k/k-1}$$

得到5个公式

$$\{\begin{array}{ll}{\hat{X}}_{k/k-1}={\Phi }_{k/k-1}{\hat{X}}_{k-1}& 状态一步预测\\ P_{k/k-1}={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T& 状态一步预测均方差阵\\ K_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T-R_k{)}^{-1}& 滤波增益\\ {\hat{X}}_k=(I-K_k{H}_k){\hat{X}}_{k/k-1}+K_k{Z}_k& 状态估计\\ P_k=(I-K_k{H}_k)P_{k/k-1}& 状态估计均方误差阵\end{array}$$
$K_k$的等价公式
$$\{\begin{array}{ll}{K}_k=P_{k/k-1}{H}_k^T(H_k{P}_{k/k-1}{H}_k^T-R_k{)}^{-1}& 滤波增益\\ K_k=P_k{H}_k^T{R}_k^{-1}& \end{array}$$
$P_k$的等价公式
$$\{\begin{array}{l}{P}_k=(I-K_k{H}_k)P_{k/k-1}\\ P_k=P_{k/k-1}-K_k(H_k{P}_{k/k-1}{H}_k^T+R_k)K_k^T\\ P_k=(I-K_k{H}_k)P_{k/k-1}(I-K_k{H}_k{)}^T+K_k{R}_k{K}_k^T(Jose算法,数值对称性和稳定相对比前两个较好)\\ P_k^{-1}=P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k(存在多个求逆运算不推荐使用)\end{array}$$
一个实例讲解