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

什么是状态空间

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

随机系统状态空间模型

系统状态空间
X k : n 维 状 态 向 量 X_k:n维状态向量 Xk​:n维状态向量
Z k : m 维 测 量 向 量 Z_k:m维测量向量 Zk​:m维测量向量
Φ k / k − 1 : 已 知 的 系 统 结 构 参 数 \Phi_{k/k-1}:已知的系统结构参数 Φk/k−1​:已知的系统结构参数
Γ k / k − 1 : 已 知 的 系 统 结 构 参 数 ， 分 别 为 n × l 阶 系 统 分 配 噪 声 \Gamma_{k/k-1}:已知的系统结构参数，分别为n×l阶系统分配噪声 Γk/k−1​:已知的系统结构参数，分别为n×l阶系统分配噪声
H k : 已 知 的 系 统 结 构 参 数 ， 分 别 为 m × n 阶 测 量 矩 阵 H_k:已知的系统结构参数，分别为m×n阶测量矩阵 Hk​:已知的系统结构参数，分别为m×n阶测量矩阵
V k : m 维 测 量 噪 声 ， 高 斯 白 噪 声 ， 服 从 正 太 分 布 V_k:m维测量噪声，高斯白噪声，服从正太分布 Vk​:m维测量噪声，高斯白噪声，服从正太分布
W k − 1 : m 维 系 统 噪 声 向 量 ， 高 斯 白 噪 声 ， 服 从 正 太 分 布 W_{k-1}:m维系统噪声向量，高斯白噪声，服从正太分布 Wk−1​:m维系统噪声向量，高斯白噪声，服从正太分布
V k 与 W k − 1 互 不 相 关 V_k与W_{k-1}互不相关 Vk​与Wk−1​互不相关
{ X k = Φ k / k − 1 X k − 1 + Γ k / k − 1 W k − 1 Z k = H k X k + V k s t . { E [ W k ] = 0 , E [ W k W j T ] = Q k δ k j Q k ≥ 0 E [ V k ] = 0 , E [ V k V j T ] = R k δ k j , E [ W k V j T ] = 0 R ≥ 0 \begin{cases} X_k=\Phi_{k/k-1}X_{k-1}+\Gamma_{k/k-1}W_{k-1}\\ Z_k=H_kX_k+V_k\\ \end{cases} \\ st. \\ \begin{cases} E[W_k]=0,E[W_kW_j^T]=Q_k\delta_{kj} &Q_k \geq 0\\ E[V_k]=0,E[V_kV_j^T]=R_k\delta_{kj},E[W_kV_j^T]=0&R\geq 0\\ \end{cases} {Xk​=Φk/k−1​Xk−1​+Γk/k−1​Wk−1​Zk​=Hk​Xk​+Vk​​st.{E[Wk​]=0,E[Wk​WjT​]=Qk​δkj​E[Vk​]=0,E[Vk​VjT​]=Rk​δkj​,E[Wk​VjT​]=0​Qk​≥0R≥0​

滤波方程的推导

k-1时刻系统的观测方程和系统方程
使用最小方差估计：
X ^ k − 1 : 状 态 估 计 值 \hat{X}_{k-1} :状态估计值 X^k−1​:状态估计值
X ~ k − 1 : 状 态 估 计 值 与 实 际 值 的 误 差 \tilde X_{k-1} :状态估计值与实际值的误差 X~k−1​:状态估计值与实际值的误差
P k − 1 : 均 方 误 差 P_{k-1} :均方误差 Pk−1​:均方误差
X ~ k − 1 = X k − 1 − X ^ k − 1 （ 系 统 方 程 ） P k − 1 = E [ X ~ k − 1 X ~ k − 1 T ] = E [ ( X k − 1 − X ^ k − 1 ) ( X k − 1 − X ^ k − 1 ) T ] （ 观 测 方 程 ） \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]（观测方程） X~k−1​=Xk−1​−X^k−1​（系统方程）Pk−1​=E[X~k−1​X~k−1T​]=E[(Xk−1​−X^k−1​)(Xk−1​−X^k−1​)T]（观测方程）
根据状态估计值（ X ^ k − 1 \hat{X}_{k-1} X^k−1​） 获得系统方程最优值(0均值噪声不会对最优值造成影响)
X k / k − 1 : 最 优 估 计 值 ( 也 称 状 态 一 步 估 计 ) X_{k/k-1}:最优估计值(也称状态一步估计) Xk/k−1​:最优估计值(也称状态一步估计)
X ^ k / k − 1 = E [ Φ k / k − 1 X k − 1 + Γ k / k − 1 W k − 1 ] = Φ k / k − 1 X 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} X^k/k−1​=E[Φk/k−1​Xk−1​+Γk/k−1​Wk−1​]=Φk/k−1​Xk−1​
获得系统方程最优估计值测量误差
X ~ k / k − 1 : 获 得 最 优 估 计 值 测 量 误 差 \tilde X_{k/k-1}:获得最优估计值测量误差 X~k/k−1​:获得最优估计值测量误差
X ~ k / k − 1 = Φ k / k − 1 X k − 1 + Γ k / k − 1 W k − 1 − X ^ k / k − 1 = Φ k / k − 1 X ~ k − 1 + Γ k / k − 1 W 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} X~k/k−1​=Φk/k−1​Xk−1​+Γk/k−1​Wk−1​−X^k/k−1​=Φk/k−1​X~k−1​+Γk/k−1​Wk−1​
获得系统方程最优估计值的均方误差矩阵
X ~ k − 1 与 W k − 1 不 相 关 \tilde X_{k-1}与W_{k-1}不相关 X~k−1​与Wk−1​不相关
P k / k − 1 : 最 优 估 计 值 的 均 方 误 差 P_{k/k-1}:最优估计值的均方误差 Pk/k−1​:最优估计值的均方误差
P k / k − 1 = E [ X ~ k − 1 X ~ k − 1 T ] = E [ X ~ k − 1 X ~ k − 1 T ] = E [ ( Φ k / k − 1 X ~ k − 1 + Γ k / k − 1 W k − 1 ) ( Φ k / k − 1 X ~ k − 1 + Γ k / k − 1 W k − 1 ) T ] = Φ k / k − 1 E [ X ~ k − 1 X ~ k − 1 T ] Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 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^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1} Pk/k−1​=E[X~k−1​X~k−1T​]=E[X~k−1​X~k−1T​]=E[(Φk/k−1​X~k−1​+Γk/k−1​Wk−1​)(Φk/k−1​X~k−1​+Γk/k−1​Wk−1​)T]=Φk/k−1​E[X~k−1​X~k−1T​]Φk/k−1T​+Γk−1​Qk−1​Γk−1T​=Φk/k−1​Pk−1​Φk/k−1T​+Γk−1​Qk−1​Γk−1​
同理可得测量方程的，估计值、误差、均方差矩阵
{ Z ^ k / k − 1 = E [ H k X k + V k ] = H k X ^ k / k − 1 最 优 估 计 值 / 最 优 一 步 估 计 Z ~ k / k − 1 = Z k − Z ^ k / k − 1 = ( H k X k + V k ) − H k X ^ k / k − 1 = H k X ~ k / k − 1 + V k 最 优 估 计 值 的 误 差 / 最 优 一 步 估 计 误 差 P Z Z , k / k − 1 = E [ Z ~ k / k − 1 Z ~ k / k − 1 T ] = H k E [ X ~ k / k − 1 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 X Z , k / k − 1 = E [ X ~ k / k − 1 Z ~ k / k − 1 T ] = E [ X ~ k / k − 1 ( H k X ~ k / k − 1 + V k ) T ] = P k / k − 1 H k T 最 优 一 步 估 计 的 协 方 差 \begin{cases} \hat Z_{k/k-1}=E[H_kX_k+V_k]=H_k\hat X_{k/k-1}&最优估计值/最优一步估计\\ \tilde Z_{k/k-1}=Z_k-\hat Z_{k/k-1}=(H_kX_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_kE[\tilde X_{k/k-1}\tilde X_{k/k-1}^T]H_k^T+E[V_kV_k^T]=H_kP_{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{cases} ⎩⎪⎪⎪⎨⎪⎪⎪⎧​Z^k/k−1​=E[Hk​Xk​+Vk​]=Hk​X^k/k−1​Z~k/k−1​=Zk​−Z^k/k−1​=(Hk​Xk​+Vk​)−Hk​X^k/k−1​=Hk​X~k/k−1​+Vk​PZZ,k/k−1​=E[Z~k/k−1​Z~k/k−1T​]=Hk​E[X~k/k−1​X~k/k−1T​]HkT​+E[Vk​VkT​]=Hk​Pk/k−1​HkT​+Rk​PXZ,k/k−1​=E[X~k/k−1​Z~k/k−1T​]=E[X~k/k−1​(Hk​X~k/k−1​+Vk​)T]=Pk/k−1​HkT​​最优估计值/最优一步估计最优估计值的误差/最优一步估计误差最优一步估计的均方差最优一步估计的协方差​
修正系数 K k K_k Kk​
从上方的计算可以的得到，测量方程和系统方程的最优值，使用 Z ^ k / k − 1 （ 测 量 方 程 一 步 估 计 误 差 ） \hat Z_{k/k-1}（测量方程一步估计误差） Z^k/k−1​（测量方程一步估计误差）修正 X ^ k / k − 1 （ 系 统 方 程 一 步 估 计 值 ） \hat X_{k/k-1}（系统方程一步估计值） X^k/k−1​（系统方程一步估计值）可以进一步提高精度，即：
K k : 误 差 修 正 系 数 （ 滤 波 增 益 f i l t e r g a i n ） K_k:误差修正系数（滤波增益 filter gain） Kk​:误差修正系数（滤波增益filtergain）
Z ~ k / k − 1 : ^ 新 息 （ i n n o v a t i o n ） \tilde Z_{k/k-1}\hat:新息（innovation） Z~k/k−1​:^新息（innovation）
X ^ k = X ^ k / k − 1 + K k Z ~ k / k − 1 ( 当 前 值 = 前 一 状 态 估 计 值 + K k 测 量 预 测 误 差 ) = ( I − K k H k ) X ^ k / k − 1 + K k Z k = ( I − K k H k ) Φ k / k − 1 X ^ k − 1 + K k Z k \hat X_k=\hat X_{k/k-1}+K_k\tilde Z_{k/k-1}(当前值=前一状态估计值+K_k测量预测误差)\\ =(I-K_kH_k)\hat X_{k/k-1}+K_kZ_k \\ =(I-K_kH_k)\Phi_{k/k-1}\hat X_{k-1}+K_kZ_k X^k​=X^k/k−1​+Kk​Z~k/k−1​(当前值=前一状态估计值+Kk​测量预测误差)=(I−Kk​Hk​)X^k/k−1​+Kk​Zk​=(I−Kk​Hk​)Φk/k−1​X^k−1​+Kk​Zk​

修正系数 K k K_k Kk​的确定

k时刻系统的观测方程和系统方程
X ~ k = X k − X ^ k \tilde X_k=X_k-\hat X_k X~k​=Xk​−X^k​
X ^ k = ( I − K k H k ) X ^ k / k − 1 + K k Z k 带 入 \hat X_k=(I-K_kH_k)\hat X_{k/k-1}+K_kZ_k带入 X^k​=(I−Kk​Hk​)X^k/k−1​+Kk​Zk​带入
Z k = H k X k + V k Z_k=H_kX_k+V_k Zk​=Hk​Xk​+Vk​
X ~ k = ( I − K k H k ) X ~ x / x − 1 − K k V k \tilde X_k=(I-K_kH_k)\tilde X_{x/x-1}-K_kV_k X~k​=(I−Kk​Hk​)X~x/x−1​−Kk​Vk​
k时刻 X ^ k \hat X_k X^k​的均方误差阵
P k = E [ X ~ k X ~ k T ] = E [ ( I − K k H k ) X ~ x / x − 1 − K k V k ) ( I − K k H k ) 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 P_k=E[\tilde X_k\tilde X_k^T] \\ =E[(I-K_kH_k)\tilde X_{x/x-1}-K_kV_k)(I-K_kH_k)\tilde X_{x/x-1}-K_kV_k)^T]\\ =(I-K_kH_k)E[X_{x/x-1}X_{x/x-1}^T](I-K_kH_k)^T+K_kE[V_kV_k^T]K_k^T \\ =(I-K_kH_k)P_{k/k-1}(I-K_kH_k)^T+K_kR_kK_k^T Pk​=E[X~k​X~kT​]=E[(I−Kk​Hk​)X~x/x−1​−Kk​Vk​)(I−Kk​Hk​)X~x/x−1​−Kk​Vk​)T]=(I−Kk​Hk​)E[Xx/x−1​Xx/x−1T​](I−Kk​Hk​)T+Kk​E[Vk​VkT​]KkT​=(I−Kk​Hk​)Pk/k−1​(I−Kk​Hk​)T+Kk​Rk​KkT​
k时刻估计误差最小
E [ X ~ k X ~ k T ] ∣ m i n = t r ( P k ) m i n = t r ( ( I − K k H k ) P k / k − 1 ( I − K k H k ) T + K k R k K k T ) = t r ( P k / k − 1 ) − t r ( K k H k P k / k − 1 ) − t r ( ( K k H k P k / k − 1 ) T ) + t r ( K k ( H k P k / k − 1 H k T + R k ) K k T ) E[\tilde X_k\tilde X_k^T]|_{min}=tr(P_k)_{min} \\ =tr((I-K_kH_k)P_{k/k-1}(I-K_kH_k)^T+K_kR_kK_k^T)\\ =tr(P_{k/k-1})-tr(K_kH_kP_{k/k-1})-tr((K_kH_kP_{k/k-1})^T)+tr(K_k(H_kP_{k/k-1}H_k^T+R_k)K_k^T) E[X~k​X~kT​]∣min​=tr(Pk​)min​=tr((I−Kk​Hk​)Pk/k−1​(I−Kk​Hk​)T+Kk​Rk​KkT​)=tr(Pk/k−1​)−tr(Kk​Hk​Pk/k−1​)−tr((Kk​Hk​Pk/k−1​)T)+tr(Kk​(Hk​Pk/k−1​HkT​+Rk​)KkT​)
k时刻估计误差求极小值，即导数值为0的点
已知两个方阵迹的求导等式：
d d X t r ( X B ) = d d X t r ( ( X B ) T ) = B T \frac{d}{dX}tr(XB)=\frac{d}{dX}tr((XB)^T)=B^T dXd​tr(XB)=dXd​tr((XB)T)=BT
d d X t r ( X A X T ) = 2 X A \frac{d}{dX}tr(XAX^T)=2XA dXd​tr(XAXT)=2XA
d d K k t r ( P ) = 0 − ( H k P k / k − 1 ) T − ( H k P k / k − 1 ) T + 2 K 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 \frac{d}{dK_k}tr(P)=0-(H_kP_{k/k-1})^T-(H_kP_{k/k-1})^T+2K_k(H_kP_{k/k-1}H_k^T+R_k)\\ =2[K_k(H_kP_{k/k-1}H_k^T+R_k)-P_{k/k-1}H_k^T]=0 dKk​d​tr(P)=0−(Hk​Pk/k−1​)T−(Hk​Pk/k−1​)T+2Kk​(Hk​Pk/k−1​HkT​+Rk​)=2[Kk​(Hk​Pk/k−1​HkT​+Rk​)−Pk/k−1​HkT​]=0
解得 K k K_k Kk​
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/k-1}H_k^T(H_kP_{k/k-1}H_k^T-R_k)^{-1} Kk​=Pk/k−1​HkT​(Hk​Pk/k−1​HkT​−Rk​)−1
P k = ( I − K k H k ) P k / k − 1 P_k=(I-K_kH_k)P_{k/k-1} Pk​=(I−Kk​Hk​)Pk/k−1​

得到5个公式

{ X ^ k / k − 1 = Φ k / k − 1 X ^ k − 1 状 态 一 步 预 测 P k / k − 1 = Φ k / k − 1 P k − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T 状 态 一 步 预 测 均 方 差 阵 K k = P k / k − 1 H k T ( H k P k / k − 1 H k T − R k ) − 1 滤 波 增 益 X ^ k = ( I − K k H k ) X ^ k / k − 1 + K k Z k 状 态 估 计 P k = ( I − K k H k ) P k / k − 1 状 态 估 计 均 方 误 差 阵 \begin{cases} \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^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T&状态一步预测均方差阵\\ K_k=P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T-R_k)^{-1}&滤波增益\\ \hat X_k=(I-K_kH_k)\hat X_{k/k-1}+K_kZ_k&状态估计\\ P_k=(I-K_kH_k)P_{k/k-1}&状态估计均方误差阵\\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧​X^k/k−1​=Φk/k−1​X^k−1​Pk/k−1​=Φk/k−1​Pk−1​Φk/k−1T​+Γk−1​Qk−1​Γk−1T​Kk​=Pk/k−1​HkT​(Hk​Pk/k−1​HkT​−Rk​)−1X^k​=(I−Kk​Hk​)X^k/k−1​+Kk​Zk​Pk​=(I−Kk​Hk​)Pk/k−1​​状态一步预测状态一步预测均方差阵滤波增益状态估计状态估计均方误差阵​
K k K_k Kk​的等价公式
{ 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 \begin{cases} K_k=P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T-R_k)^{-1}&滤波增益\\ K_k=P_kH^T_kR_k^{-1}\\ \end{cases} {Kk​=Pk/k−1​HkT​(Hk​Pk/k−1​HkT​−Rk​)−1Kk​=Pk​HkT​Rk−1​​滤波增益
P k P_k Pk​的等价公式
{ 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 ( J o s e 算 法 , 数 值 对 称 性 和 稳 定 相 对 比 前 两 个 较 好 ) P k − 1 = P k / k − 1 − 1 + H k T R k − 1 H k ( 存 在 多 个 求 逆 运 算 不 推 荐 使 用 ) \begin{cases} P_k=(I-K_kH_k)P_{k/k-1}\\ P_k=P_{k/k-1}-K_k(H_kP_{k/k-1}H_{k}^T+R_k)K_k^T\\ P_k=(I-K_kH_k)P_{k/k-1}(I-K_kH_k)^T+K_kR_kK_k^T(Jose算法,数值对称性和稳定相对比前两个较好)\\ P_k^{-1}=P_{k/k-1}^{-1}+H_k^TR_k^{-1}H_k(存在多个求逆运算不推荐使用)\\ \end{cases} ⎩⎪⎪⎪⎨⎪⎪⎪⎧​Pk​=(I−Kk​Hk​)Pk/k−1​Pk​=Pk/k−1​−Kk​(Hk​Pk/k−1​HkT​+Rk​)KkT​Pk​=(I−Kk​Hk​)Pk/k−1​(I−Kk​Hk​)T+Kk​Rk​KkT​(Jose算法,数值对称性和稳定相对比前两个较好)Pk−1​=Pk/k−1−1​+HkT​Rk−1​Hk​(存在多个求逆运算不推荐使用)​
一个实例讲解

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！