噪声相关条件下的Kalman滤波
理想状态下的kalman需要系统噪声与测量噪声之间部不相关，如果测量噪声与系统噪声相关需要进行处理
噪声相关条件下的系统状态方程
Xk:n维状态向量X_k:n维状态向量Xk:n维状态向量
Zk: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阶系统分配噪声
Hk:已知的系统结构参数，分别为m×n阶测量矩阵H_k:已知的系统结构参数，分别为m×n阶测量矩阵Hk:已知的系统结构参数，分别为m×n阶测量矩阵
Vk:m维测量噪声，高斯白噪声，服从正太分布V_k:m维测量噪声，高斯白噪声，服从正太分布Vk:m维测量噪声，高斯白噪声，服从正太分布
Wk−1:m维系统噪声向量，高斯白噪声，服从正太分布W_{k-1}:m维系统噪声向量，高斯白噪声，服从正太分布Wk−1:m维系统噪声向量，高斯白噪声，服从正太分布
Vk与Wk−1相关V_k与W_{k-1}相关Vk与Wk−1相关
{Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vkst.{E[Wk]=0,E[WkWjT]=QkδkjQk≥0系统方程E[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=SkδkjR≥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]=S_k\delta_{kj}&R\geq 0&测量方程\\ \end{cases} {Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vkst.{E[Wk]=0,E[WkWjT]=QkδkjE[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=SkδkjQk≥0R≥0系统方程测量方程
## 噪声相关条件下的Kalman滤波
重新构造测量方程，使测量误差与系统误差协方差为0
构造矩阵Jk−1J_{k-1}Jk−1满足如下条件:
0=Jk−1(Zk−1−Hk−1Xk−1−Vk−1)0=J_{k-1}(Z_{k-1}-H_{k-1}X_{k-1}-V_{k-1})0=Jk−1(Zk−1−Hk−1Xk−1−Vk−1)
将条件带入系统方程：
Xk=Φk/k−1Xk−1+Γk/k−1Wk−1+Jk−1(Zk−1−Hk−1Xk−1−Vk−1)=Φk/k−1∗Xk−1+Jk−1Zk−1+Wk−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}^*Xk=Φk/k−1Xk−1+Γk/k−1Wk−1+Jk−1(Zk−1−Hk−1Xk−1−Vk−1)=Φk/k−1∗Xk−1+Jk−1Zk−1+Wk−1∗
得到:
{Xk=Φk/k−1∗Xk−1+Jk−1Zk−1+Wk−1∗Φk/k−1∗=Φk/k−1−Jk−1Hk−1Wk−1∗=Γk−1Wk−1−Jk−1Vk−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} ⎩⎪⎨⎪⎧Xk=Φk/k−1∗Xk−1+Jk−1Zk−1+Wk−1∗Φk/k−1∗=Φk/k−1−Jk−1Hk−1Wk−1∗=Γk−1Wk−1−Jk−1Vk−1
求期望：
{E[Wk∗]=E[Γk−1Wk−1−Jk−1Vk−1]=0E[Wk∗Wj∗T]=(ΓkQkΓkT+JkRkJkT−ΓkSkJkT−JkSkTΓkT)δkjE[Wk∗VjT]=(ΓkSk−JkRk)δkj\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[Wk∗]=E[Γk−1Wk−1−Jk−1Vk−1]=0E[Wk∗Wj∗T]=(ΓkQkΓkT+JkRkJkT−ΓkSkJkT−JkSkTΓkT)δkjE[Wk∗VjT]=(ΓkSk−JkRk)δkj
要使测量误差与系统误差不相关：
E[Wk∗VjT]=(ΓkSk−JkRk)δkj=0E[W_k^*V_j^T]=(\Gamma_kS_k-J_kR_k)\delta_{kj}=0E[Wk∗VjT]=(ΓkSk−JkRk)δkj=0
求得SkS_kSk
Sk=ΓkSkRk−1S_k=\Gamma_kS_kR_k^{-1}Sk=ΓkSkRk−1
重新计算方差阵为：
E[Wk∗Wj∗T]=(ΓkQkΓkT−JkRkJkT)δkjE[W_k^*W_j^{*T}]=(\Gamma_kQ_k\Gamma_k^T-J_kR_kJ_k^T)\delta_{kj}E[Wk∗Wj∗T]=(ΓkQkΓkT−JkRkJkT)δkj
Qk∗=ΓkQkΓkT−JkRkJkTQ_k^*=\Gamma_kQ_k\Gamma_k^T-J_kR_kJ_k^TQk∗=ΓkQkΓkT−JkRkJkT
得到新的状态空间方程为：
{Xk=Φk/k−1∗Xk−1+Jk−1Zk−1+Wk−1∗Zk=HkXk+Vkst.{E[Wk∗]=0,E[Wk∗Wj∗T]=Qk∗δkjQk∗≥0系统方程E[Vk]=0,E[VkVjT]=Rkδkj,E[Wk∗VjT]=0R≥0测量方程\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} {Xk=Φk/k−1∗Xk−1+Jk−1Zk−1+Wk−1∗Zk=HkXk+Vkst.{E[Wk∗]=0,E[Wk∗Wj∗T]=Qk∗δkjE[Vk]=0,E[VkVjT]=Rkδkj,E[Wk∗VjT]=0Qk∗≥0R≥0系统方程测量方程
得到kalman滤波方程为
{X^k/k−1=Φk/k−1∗X^k−1+Jk−1Zk−1状态一步预测Pk/k−1=Φk/k−1∗Pk−1Φk/k−1∗T+Qk−1∗状态一步预测均方差阵Kk=Pk/k−1HkT(HkPk/k−1HkT−Rk)−1滤波增益X^k=(I−KkHk)X^k/k−1+KkZk状态估计Pk=(I−KkHk)Pk/k−1状态估计均方误差阵\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}⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧X^k/k−1=Φk/k−1∗X^k−1+Jk−1Zk−1Pk/k−1=Φk/k−1∗Pk−1Φk/k−1∗T+Qk−1∗Kk=Pk/k−1HkT(HkPk/k−1HkT−Rk)−1X^k=(I−KkHk)X^k/k−1+KkZkPk=(I−KkHk)Pk/k−1状态一步预测状态一步预测均方差阵滤波增益状态估计状态估计均方误差阵
系统噪声为有色噪声的Kalman滤波
系统噪声为有色噪声的系统状态方程
Xk:n维状态向量X_k:n维状态向量Xk:n维状态向量
Zk: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阶系统分配噪声
Hk:已知的系统结构参数，分别为m×n阶测量矩阵H_k:已知的系统结构参数，分别为m×n阶测量矩阵Hk:已知的系统结构参数，分别为m×n阶测量矩阵
Vk:m维测量噪声，高斯白噪声，服从正太分布V_k:m维测量噪声，高斯白噪声，服从正太分布Vk:m维测量噪声，高斯白噪声，服从正太分布
Wk−1:m维系统噪声向量，有色噪声W_{k-1}:m维系统噪声向量，有色噪声Wk−1:m维系统噪声向量，有色噪声
Vk与Wk−1不相关:ΓkWk=[Γw,kΓc,k][Ww,kWc,k]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]Vk与Wk−1不相关:ΓkWk=[Γw,kΓc,k][Ww,kWc,k]
{Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vkst.{E[Ww,k]=0,E[Ww,kWw,jT]=Qw,kδkj,E[Ww,kWc,jT]=0Qk≥0系统方程E[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0R≥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_{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} {Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vkst.{E[Ww,k]=0,E[Ww,kWw,jT]=Qw,kδkj,E[Ww,kWc,jT]=0E[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0Qk≥0R≥0系统方程测量方程
系统噪声表示为有限维状态
Wc,k′:为Wc,k的拓展维数W_{c,k}' :为W_{c,k}的拓展维数Wc,k′:为Wc,k的拓展维数
ξc,k:零均值高斯白噪声\xi_{c,k} :零均值高斯白噪声ξc,k:零均值高斯白噪声
[Wc,kWc,k′]=[Πk/k−111Πk/k−112Πk/k−121Πk/k−122][Wc,k−1Wc,k−1′]+[ξc,kξc,k′]E[[ξkξk′]]=0,E[[ξkξk′][ξjξj′]T]=[Qc,k00Qc,k′]δk,j\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}[Wc,kWc,k′]=[Πk/k−111Πk/k−121Πk/k−112Πk/k−122][Wc,k−1Wc,k−1′]+[ξc,kξc,k′]E[[ξkξk′]]=0,E[[ξkξk′][ξjξj′]T]=[Qc,k00Qc,k′]δk,j
系统噪声表示为有限维状态带入状态空间
{Xka=Φk/k−1aXk−1a+Γk/k−1aWk−1aZk=HkaXka+Vkst.{E[Ww,ka]=0,E[Ww,kaWw,jaT]=Qw,kδkj=diag(Qw,k,Qc,k,Qc,k′)Qk≥0系统方程E[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0R≥0测量方程\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} {Xka=Φk/k−1aXk−1a+Γk/k−1aWk−1aZk=HkaXka+Vkst.{E[Ww,ka]=0,E[Ww,kaWw,jaT]=Qw,kδkj=diag(Qw,k,Qc,k,Qc,k′)E[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0Qk≥0R≥0系统方程测量方程
Xka=[XkWc,kWc,k′],Φk/k−1a=[Φk/k−1Γc,k−100Πk/k−1k/k−111Πk/k−1k/k−1120Πk/k−1k/k−121Πk/k−1k/k−122]Γka=[Γw,k000I000I],Wka=[Ww,kξkξk′],Hka=[Hk00]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]Xka=⎣⎡XkWc,kWc,k′⎦⎤,Φk/k−1a=⎣⎡Φk/k−100Γc,k−1Πk/k−1k/k−111Πk/k−1k/k−1210Πk/k−1k/k−112Πk/k−1k/k−122⎦⎤Γka=⎣⎡Γw,k000I000I⎦⎤,Wka=⎣⎡Ww,kξkξk′⎦⎤,Hka=[Hk00]
测量噪声为有色噪声的Kalman滤波
将有色噪声白化，假设测量误差VkV_kVk仅有一部分分量是白噪声
{Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vk\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} \\ {Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vk
状态增广法
Vk=[Θw,kΘc,k][Vw,kVc,k]E[Vw,k]=0,E[Vw,kVw,jT]=Rw,kδkj,E[Vw,kVc,jT]=0V_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]=0Vk=[Θw,kΘc,k][Vw,kVc,k]E[Vw,k]=0,E[Vw,kVw,jT]=Rw,kδkj,E[Vw,kVc,jT]=0
系统噪声表示为有限维状态
[Vc,kVc,k′]=[Ψk/k−111Ψk/k−112Ψk/k−121Ψk/k−122][Vc,k−1Vc,k−1′]+[ξc,kξc,k′]E[[ξkξk′]]=0,E[[ξkξk′][ξjξj′]T]=[Rc,k00Rc,k′]δk,j\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}[Vc,kVc,k′]=[Ψk/k−111Ψk/k−121Ψk/k−112Ψk/k−122][Vc,k−1Vc,k−1′]+[ξc,kξc,k′]E[[ξkξk′]]=0,E[[ξkξk′][ξjξj′]T]=[Rc,k00Rc,k′]δk,j
测量噪声表示为有限维状态带入状态空间
{Xka=Φk/k−1aXk−1a+Γk/k−1aWk−1aZk=HkaXka+Vkst.{E[Ww,ka]=0,E[Ww,kaWw,jaT]=Qw,kδkj=diag(Qk,Rc,k−1,Rc,k−1′)E[Vka]=0,E[VkaVjaT]=(Θw,kRw,kΘw,kT+Θc,kRc,k−1Θc,kT),E[WkVjaT]=diag(0,(Rc,k−1Θc,kT),0)δkjR≥0测量方程\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} {Xka=Φk/k−1aXk−1a+Γk/k−1aWk−1aZk=HkaXka+Vkst.{E[Ww,ka]=0,E[Ww,kaWw,jaT]=Qw,kδkj=diag(Qk,Rc,k−1,Rc,k−1′)E[Vka]=0,E[VkaVjaT]=(Θw,kRw,kΘw,kT+Θc,kRc,k−1Θc,kT),E[WkVjaT]=diag(0,(Rc,k−1Θc,kT),0)δkjR≥0测量方程
Xka=[XkVc,k−1Vc,k−1′],Φk/k−1a=[Φk/k−1Γc,k−100Ψk/k−1k/k−211Ψk/k−1k/k−2120Ψk/k−1k/k−221Ψk/k−1k/k−222]Γka=[Γk000I000I],Wka=[Ww,kξk−1ξk−1′],Hka=[Hkξk−1ξk−1′],Vka=Θw,kVw.k+Θc,kξk−1X_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}Xka=⎣⎡XkVc,k−1Vc,k−1′⎦⎤,Φk/k−1a=⎣⎡Φk/k−100Γc,k−1Ψk/k−1k/k−211Ψk/k−1k/k−2210Ψk/k−1k/k−212Ψk/k−1k/k−222⎦⎤Γka=⎣⎡Γk000I000I⎦⎤,Wka=⎣⎡Ww,kξk−1ξk−1′⎦⎤,Hka=[Hkξk−1ξk−1′],Vka=Θw,kVw.k+Θc,kξk−1
测量求差法
如果测量方程中的有色误差VkV_kVk可以表示为如下形式:
ξk−1:零均值高斯误差,E[ξk]=0,E[ξkξjT]=Rξ,kδkj\xi_{k-1}:零均值高斯误差,E[\xi_k]=0,E[\xi_k\xi_j^T]=R_{\xi,k}\delta_{kj}ξk−1:零均值高斯误差,E[ξk]=0,E[ξkξjT]=Rξ,kδkj
Vk=ψk/k−1Vk−1+ξk−1V_k=\psi_{k/k-1}V_{k-1}+\xi_{k-1}Vk=ψk/k−1Vk−1+ξk−1
将相邻时刻的量求差并展开
Zk−ψk/k−1Zk/k−1=[Hk(Φk/k−1Xk−1+Γk/k−1Wk−1)+(ψk/k−1Vk−1+ξk−1)]−ψk/k−1(Hk−1Xk−1+Vk−1)=(HkΦk/k−1−ψk/k−1Hk−1)Xk−1+(HkΓk/k−1Wk−1+ξ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})Zk−ψk/k−1Zk/k−1=[Hk(Φk/k−1Xk−1+Γk/k−1Wk−1)+(ψk/k−1Vk−1+ξk−1)]−ψk/k−1(Hk−1Xk−1+Vk−1)=(HkΦk/k−1−ψk/k−1Hk−1)Xk−1+(HkΓk/k−1Wk−1+ξk−1)
得到测量误差与系统误差相关的观测模型
{Xk∗=Φk/k−1∗Xk−1∗+Γk/k−1∗Wk−1∗Zk∗=Hk∗Xk∗+Vk∗st:{E[Wk∗]=0,E[Wk∗(Wj∗)T]=Qk−1δkjE[Vk∗]=0,E[Vk∗(Wj∗)T]=(HkΓk−1Qk−1Γk−1THkT+Rξ,k−1)δkjE[Wk∗Wj∗T]=Qk−1Γk−1THkTδkj\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} \\ {Xk∗=Φk/k−1∗Xk−1∗+Γk/k−1∗Wk−1∗Zk∗=Hk∗Xk∗+Vk∗st:⎩⎪⎨⎪⎧E[Wk∗]=0,E[Wk∗(Wj∗)T]=Qk−1δkjE[Vk∗]=0,E[Vk∗(Wj∗)T]=(HkΓk−1Qk−1Γk−1THkT+Rξ,k−1)δkjE[Wk∗Wj∗T]=Qk−1Γk−1THkTδkj
Zk∗=Zk−ψk/k−1Zk/k−1,Hk∗=HkΦk/k−1−ψk/k−1Hk−1,Vk∗=HkΓk/k−1Wk−1+ξk−1Xk∗=Xk−1,Φk/k−1∗=Φk−1/k−2,Γk∗=Γk−1,Wk∗=Wk−1Z_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}Zk∗=Zk−ψk/k−1Zk/k−1,Hk∗=HkΦk/k−1−ψk/k−1Hk−1,Vk∗=HkΓk/k−1Wk−1+ξk−1Xk∗=Xk−1,Φk/k−1∗=Φk−1/k−2,Γk∗=Γk−1,Wk∗=Wk−1
