捷联惯导系统学习6.1(一些卡尔曼滤波处理技术 )

最新推荐文章于 2024-04-25 12:54:39 发布

磊磊哈哈

最新推荐文章于 2024-04-25 12:54:39 发布

阅读量527

点赞数

本文链接：https://blog.csdn.net/weixin_37781153/article/details/108862504

版权

捷联惯导与组合导航专栏收录该内容

47 篇文章 76 订阅

订阅专栏

噪声相关条件下的Kalman滤波

理想状态下的kalman需要系统噪声与测量噪声之间部不相关，如果测量噪声与系统噪声相关需要进行处理

噪声相关条件下的系统状态方程
$X_k:n维状态向量$
$Z_k:m维测量向量$
$\Phi_{k/k-1}:已知的系统结构参数$
$\Gamma_{k/k-1}:已知的系统结构参数，分别为n×l阶系统分配噪声$
$H_k:已知的系统结构参数，分别为m×n阶测量矩阵$
$V_k:m维测量噪声，高斯白噪声，服从正太分布$
$W_{k-1}:m维系统噪声向量，高斯白噪声，服从正太分布$
$V_k与W_{k-1}相关$
$\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]=S_k\delta_{kj}&R\geq 0&测量方程\\ \end{cases}$
## 噪声相关条件下的Kalman滤波
重新构造测量方程，使测量误差与系统误差协方差为0
构造矩阵 $J_{k-1}$ 满足如下条件:
$0=J_{k-1}(Z_{k-1}-H_{k-1}X_{k-1}-V_{k-1})$
将条件带入系统方程：
$X_k=\Phi_{k/k-1}X_{k-1}+\Gamma_{k/k-1}W_{k-1}+J_{k-1}(Z_{k-1}-H_{k-1}X_{k-1}-V_{k-1}) \\ =\Phi^*_{k/k-1}X_{k-1}+J_{k-1}Z_{k-1}+W_{k-1}^*$
得到:
$\begin{cases} X_k=\Phi^*_{k/k-1}X_{k-1}+J_{k-1}Z_{k-1}+W_{k-1}^*\\ \Phi^*_{k/k-1}=\Phi_{k/k-1}-J_{k-1}H_{k-1}\\ W_{k-1}^*=\Gamma_{k-1}W_{k-1}-J_{k-1}V_{k-1}\\ \end{cases}$
求期望：
$\begin{cases} E[W_k^*]=E[\Gamma_{k-1}W_{k-1}-J_{k-1}V_{k-1}]=0\\ E[W_k^*W_j^{*T}]=(\Gamma_kQ_k\Gamma^T_k+J_kR_kJ_k^T-\Gamma_kS_kJ_k^T-J_kS_k^T\Gamma_k^T)\delta_{kj}\\ E[W_k^*V_j^T]=(\Gamma_kS_k-J_kR_k)\delta_{kj}\\ \end{cases}$
要使测量误差与系统误差不相关：
$E[W_k^*V_j^T]=(\Gamma_kS_k-J_kR_k)\delta_{kj}=0$
求得 $S_k$
$S_k=\Gamma_kS_kR_k^{-1}$
重新计算方差阵为：
$E[W_k^*W_j^{*T}]=(\Gamma_kQ_k\Gamma_k^T-J_kR_kJ_k^T)\delta_{kj}$
$Q_k^*=\Gamma_kQ_k\Gamma_k^T-J_kR_kJ_k^T$
得到新的状态空间方程为：
$\begin{cases} X_k=\Phi_{k/k-1}^*X_{k-1}+J_{k-1}Z_{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_k^*V_j^T]=0&R\geq 0&测量方程\\ \end{cases}$
得到kalman滤波方程为
$\begin{cases} \hat X_{k/k-1}=\Phi_{k/k-1}^*\hat X_{k-1}+J_{k-1}Z_{k-1}&状态一步预测\\ P_{k/k-1}=\Phi^*_{k/k-1}P_{k-1}\Phi^{*T}_{k/k-1}+Q_{k-1}^*&状态一步预测均方差阵\\ 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}$

系统噪声为有色噪声的Kalman滤波

系统噪声为有色噪声的系统状态方程
$X_k:n维状态向量$
$Z_k:m维测量向量$
$\Phi_{k/k-1}:已知的系统结构参数$
$\Gamma_{k/k-1}:已知的系统结构参数，分别为n×l阶系统分配噪声$
$H_k:已知的系统结构参数，分别为m×n阶测量矩阵$
$V_k:m维测量噪声，高斯白噪声，服从正太分布$
$W_{k-1}:m维系统噪声向量，有色噪声$
$V_k与W_{k-1}不相关:\Gamma_{k}W_{k}= \left[\begin{matrix}\Gamma_{w,k}&\Gamma_{c,k} \end{matrix}\right] \left[\begin{matrix}W_{w,k}\\W_{c,k} \end{matrix}\right]$
$\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_{w,k}]=0,E[W_{w,k}W_{w,j}^T]=Q_{w,k}\delta_{kj},E[W_{w,k}W_{c,j}^T]=0&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}$
系统噪声表示为有限维状态
$W_{c,k}' :为W_{c,k}的拓展维数$
$\xi_{c,k} :零均值高斯白噪声$
$\left[\begin{matrix} W_{c,k}\\ W_{c,k}' \end{matrix}\right]=\left[\begin{matrix} \Pi_{k/k-1}^{11}&\Pi_{k/k-1}^{12}\\ \Pi_{k/k-1}^{21}&\Pi_{k/k-1}^{22} \end{matrix}\right] \left[\begin{matrix}W_{c,k-1}\\ W_{c,k-1}' \end{matrix}\right]+ \left[\begin{matrix} \xi_{c,k}\\ \xi_{c,k}' \end{matrix}\right]\\ E[\left[\begin{matrix} \xi_{k}\\ \xi_{k}' \end{matrix}\right]]=0,E[\left[\begin{matrix} \xi_{k}\\ \xi_{k}' \end{matrix}\right]\left[\begin{matrix} \xi_{j}\\ \xi_{j}' \end{matrix}\right]^T]=\left[\begin{matrix} Q_{c,k}&0\\ 0&Q_{c,k}' \end{matrix}\right]\delta_{k,j}$

系统噪声表示为有限维状态带入状态空间
$\begin{cases} X_k^a=\Phi_{k/k-1}^aX^a_{k-1}+\Gamma^a_{k/k-1}W^a_{k-1}&\\ Z_k=H^a_kX^a_k+V_k\\ \end{cases} \\ st. \\ \begin{cases} E[W^a_{w,k}]=0,E[W^a_{w,k}W_{w,j}^{aT}]=Q_{w,k}\delta_{kj}=diag(Q_{w,k},Q_{c,k},Q_{c,k}')&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}$
$X_k^a=\left[\begin{matrix} X_k\\ W_{c,k}\\ W_{c,k}' \end{matrix}\right],\Phi_{k/k-1}^a= \left[\begin{matrix} \Phi_{k/k-1}&\Gamma_{c,k-1}&0\\ 0&\Pi_{k/k-1 \ k/k-1}^{11}&\Pi_{k/k-1 \ k/k-1}^{12}\\ 0&\Pi_{k/k-1 \ k/k-1}^{21}&\Pi_{k/k-1 \ k/k-1}^{22}\\ \end{matrix}\right] \\ \Gamma_k^a=\left[\begin{matrix} \Gamma_{w,k}&0&0\\ 0&I&0\\ 0&0&I \end{matrix}\right],W_k^a= \left[\begin{matrix} W_{w,k}\\\xi_k\\\xi_k' \end{matrix}\right],H_k^a= \left[\begin{matrix} H_k&0&0\\ \end{matrix}\right]$

测量噪声为有色噪声的Kalman滤波

将有色噪声白化，假设测量误差 $V_k$ 仅有一部分分量是白噪声
$\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} \\$
状态增广法
$V_k= \left[\begin{matrix}\Theta_{w,k}&\Theta_{c,k} \end{matrix}\right] \left[\begin{matrix}V_{w,k}\\V_{c,k} \end{matrix}\right] \\ E[V_{w,k}]=0,E[V_{w,k}V_{w,j}^T]=R_{w,k}\delta_{kj},E[V_{w,k}V_{c,j}^T]=0$
系统噪声表示为有限维状态
$\left[\begin{matrix} V_{c,k}\\ V_{c,k}' \end{matrix}\right]=\left[\begin{matrix} \Psi_{k/k-1}^{11}&\Psi_{k/k-1}^{12}\\ \Psi_{k/k-1}^{21}&\Psi_{k/k-1}^{22} \end{matrix}\right] \left[\begin{matrix}V_{c,k-1}\\ V_{c,k-1}' \end{matrix}\right]+ \left[\begin{matrix} \xi_{c,k}\\ \xi_{c,k}' \end{matrix}\right]\\ E[\left[\begin{matrix} \xi_{k}\\ \xi_{k}' \end{matrix}\right]]=0,E[\left[\begin{matrix} \xi_{k}\\ \xi_{k}' \end{matrix}\right]\left[\begin{matrix} \xi_{j}\\ \xi_{j}' \end{matrix}\right]^T]=\left[\begin{matrix} R_{c,k}&0\\ 0&R_{c,k}' \end{matrix}\right]\delta_{k,j}$
测量噪声表示为有限维状态带入状态空间
$\begin{cases} X_k^a=\Phi_{k/k-1}^aX^a_{k-1}+\Gamma^a_{k/k-1}W^a_{k-1}&\\ Z_k=H^a_kX^a_k+V_k\\ \end{cases} \\ st. \\ \begin{cases} E[W^a_{w,k}]=0,E[W^a_{w,k}W_{w,j}^{aT}]=Q_{w,k}\delta_{kj}=diag(Q_{k},R_{c,k-1},R_{c,k-1}')\\ E[V_k^a]=0,E[V^a_kV_j^{aT}]=(\Theta_{w,k}R_{w,k}\Theta_{w,k}^T+\Theta_{c,k}R_{c,k-1}\Theta_{c,k}^T),E[W_kV_j^{aT}]=diag(0,(R_{c,k-1}\Theta_{c,k}^T),0)\delta_{kj}&R\geq 0&测量方程\\ \end{cases}$
$X_k^a=\left[\begin{matrix} X_k\\ V_{c,k-1}\\ V_{c,k-1}' \end{matrix}\right],\Phi_{k/k-1}^a= \left[\begin{matrix} \Phi_{k/k-1}&\Gamma_{c,k-1}&0\\ 0&\Psi_{k/k-1 \ k/k-2}^{11}&\Psi_{k/k-1 \ k/k-2}^{12}\\ 0&\Psi_{k/k-1 \ k/k-2}^{21}&\Psi_{k/k-1 \ k/k-2}^{22}\\ \end{matrix}\right] \\ \Gamma_k^a=\left[\begin{matrix} \Gamma_{k}&0&0\\ 0&I&0\\ 0&0&I \end{matrix}\right],W_k^a= \left[\begin{matrix} W_{w,k}\\\xi_{k-1}\\\xi_{k-1}' \end{matrix}\right],H_k^a= \left[\begin{matrix} H_k&\xi_{k-1}&\xi_{k-1}'\\ \end{matrix}\right],V_k^a=\Theta_{w,k}V_{w.k}+\Theta_{c,k}\xi_{k-1}$
测量求差法
如果测量方程中的有色误差 $V_k$ 可以表示为如下形式:
$\xi_{k-1}:零均值高斯误差,E[\xi_k]=0,E[\xi_k\xi_j^T]=R_{\xi,k}\delta_{kj}$
$V_k=\psi_{k/k-1}V_{k-1}+\xi_{k-1}$
将相邻时刻的量求差并展开
$Z_k-\psi_{k/k-1}Z_{k/k-1}=[H_k(\Phi_{k/k-1}X_{k-1}+\Gamma_{k/k-1}W_{k-1})+(\psi_{k/k-1}V_{k-1}+\xi_{k-1})]-\psi_{k/k-1}(H_{k-1}X_{k-1}+V_{k-1}) \\ =(H_k\Phi_{k/k-1}-\psi_{k/k-1}H_{k-1})X_{k-1}+(H_k\Gamma_{k/k-1}W_{k-1}+\xi_{k-1})$
得到测量误差与系统误差相关的观测模型
$\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_k^*(W_j^*)^T]=Q_{k-1}\delta_{kj}\\ E[V_k^*]=0,E[V_k^*(W_j^*)^T]=(H_k\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^TH_k^T+R_{\xi,k-1})\delta_{kj}\\ E[W_k^*W_{j}^{*T}]=Q_{k-1}\Gamma_{k-1}^TH_k^T\delta_{kj}\\ \end{cases} \\$
$Z_k^*=Z_k-\psi_{k/k-1}Z_{k/k-1},H^*_k=H_k\Phi_{k/k-1}-\psi_{k/k-1}H_{k-1},V^*_k=H_k\Gamma_{k/k-1}W_{k-1}+\xi_{k-1}\\ X^*_{k}=X_{k-1},\Phi^*_{k/k-1}=\Phi_{k-1/k-2},\Gamma_k^*=\Gamma_{k-1},W_k^*=W_{k-1}$

磊磊哈哈

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
捷联惯导系统学习6.1(一些卡尔曼滤波处理技术 )

狄拉克冲击函数系统的离散化实际应用中随机系统的建模往往往往时时间连续的，为了进行计算机仿真和kalman估计，需要对量需时间系统离散化系统的离散化 F(t),G(t):时间参数t的确定性时变矩阵F(t),G(t):时间参数t的确定性时变矩阵F(t),G(t):时间参数t的确定性时变矩阵X˙(t)=F(t)X(t)+G(t)w(t)\dot X(t)=F(t)X(t)+G(t)w(t)X˙(t)=F(t)X(t)+G(t)w(t)...
复制链接

扫一扫