在kalman滤波中定义： P k = E [ ( X k − E ^ [ X k ] ) ( X k − E ^ [ X k ] ) T ] P_k=E[(X_k-\hat E[X_k])(X_k-\hat E[X_k])^T] Pk=E[(Xk−E^[Xk])(Xk−E^[Xk])T] 当 P k → 0 P_k \rightarrow 0 Pk→0， X ^ k \hat X_k X^k接近于真实值。 在信息滤波中定义： I k : 信 息 矩 阵 I_k:信息矩阵 Ik:信息矩阵 I k = P k − 1 = E [ ( X k − E ^ [ X k ] ) ( X k − E ^ [ X k ] ) T ] − 1 I_k=P_k^{-1}=E[(X_k-\hat E[X_k])(X_k-\hat E[X_k])^T]^{-1} Ik=Pk−1=E[(Xk−E^[Xk])(Xk−E^[Xk])T]−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−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vkst.{E[Wk]=0,E[WkWjT]=QkδkjE[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0Qk≥0R≥0 kalman滤波方程 { 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 状 态 一 步 预 测 均 方 差 阵 K k = P k H k T R k − 1 滤 波 增 益 X ^ k = ( I − K k H k ) X ^ k / k − 1 + K k Z k 状 态 估 计 P k − 1 = P k / k − 1 − 1 + H k T R k − 1 H k 状 态 估 计 均 方 误 差 阵 \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}&状态一步预测均方差阵\\ K_k=P_kH^T_kR_k^{-1}&滤波增益\\ \hat X_k=(I-K_kH_k)\hat X_{k/k-1}+K_kZ_k&状态估计\\ P_k^{-1}=P_{k/k-1}^{-1}+H_k^TR_k^{-1}H_k&状态估计均方误差阵\\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧X^k/k−1=Φk/k−1X^k−1Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1Kk=PkHkTRk−1X^k=(I−KkHk)X^k/k−1+KkZkPk−1=Pk/k−1−1+HkTRk−1Hk状态一步预测状态一步预测均方差阵滤波增益状态估计状态估计均方误差阵 信息滤波方程 根据系统状态空间方程和kalman滤波方程得到信息增益滤波方程： { X ^ k / k − 1 = Φ k / k − 1 X ^ k − 1 状 态 一 步 预 测 I k / k − 1 = ( Φ k / k − 1 I k − 1 − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 ) − 1 状 态 一 步 预 测 均 方 差 阵 K k = P k H k T R k − 1 滤 波 增 益 X ^ k = ( I − K k H k ) X ^ k / k − 1 + K k Z k 状 态 估 计 I k = I k / k − 1 + H k T R k − 1 H k 状 态 估 计 均 方 误 差 阵 \begin{cases} \hat X_{k/k-1}=\Phi_{k/k-1}\hat X_{k-1}&状态一步预测\\ I_{k/k-1}=(\Phi_{k/k-1}I^{-1}_{k-1}\Phi^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1})^{-1}&状态一步预测均方差阵\\ K_k=P_kH^T_kR_k^{-1}&滤波增益\\ \hat X_k=(I-K_kH_k)\hat X_{k/k-1}+K_kZ_k&状态估计\\ I_k=I_{k/k-1}+H_k^TR_k^{-1}H_k&状态估计均方误差阵\\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧X^k/k−1=Φk/k−1X^k−1Ik/k−1=(Φk/k−1Ik−1−1Φk/k−1T+Γk−1Qk−1Γk−1)−1Kk=PkHkTRk−1X^k=(I−KkHk)X^k/k−1+KkZkIk=Ik/k−1+HkTRk−1Hk状态一步预测状态一步预测均方差阵滤波增益状态估计状态估计均方误差阵 为了避免 I k , I k / k − 1 I_k,I_{k/k-1} Ik,Ik/k−1存在不可逆的情况，利用矩阵求逆引力运算，进行如下转换： I k / k − 1 = ( Φ k / k − 1 I k − 1 − 1 Φ k / k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 ) − 1 = [ I − M k − 1 Γ k − 1 ( Q k − 1 − 1 + Γ k − 1 T M k − 1 Γ k − 1 ) − 1 Γ k − 1 T ] Φ k / k − 1 − T I k − 1 Φ k / k − 1 − 1 ( 推 导 略 ) = ( I − N k − 1 ) M k − 1 I_{k/k-1}=(\Phi_{k/k-1}I^{-1}_{k-1}\Phi^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1})^{-1}\\ =[I-M_{k-1}\Gamma_{k-1}(Q_{k-1}^{-1}+\Gamma_{k-1}^TM_{k-1}\Gamma_{k-1})^{-1}\Gamma_{k-1}^T]\Phi^{-T}_{k/k-1}I_{k-1}\Phi^{-1}_{k/k-1}(推导略) \\ =(I-N_{k-1})M_{k-1} Ik/k−1=(Φk/k−1Ik−1−1Φk/k−1T+Γk−1Qk−1Γk−1)−1=[I−Mk−1Γk−1(Qk−1−1+Γk−1TMk−1Γk−1)−1Γk−1T]Φk/k−1−TIk−1Φk/k−1−1(推导略)=(I−Nk−1)Mk−1 得到： { N k − 1 = M k − 1 Γ k − 1 ( Q k − 1 − 1 + Γ k − 1 T M k − 1 Γ k − 1 ) − 1 Γ k − 1 T （ 正 定 可 逆 ） M k − 1 = Φ k / k − 1 − T I k − 1 Φ k / k − 1 − 1 ( 非 负 正 定 的 ) I k / k − 1 = ( I − N k − 1 ) M k − 1 \begin{cases} N_{k-1}=M_{k-1}\Gamma_{k-1}(Q_{k-1}^{-1}+\Gamma_{k-1}^TM_{k-1}\Gamma_{k-1})^{-1}\Gamma_{k-1}^T（正定可逆）\\ M_{k-1}=\Phi^{-T}_{k/k-1}I_{k-1}\Phi^{-1}_{k/k-1}(非负正定的)\\ I_{k/k-1}=(I-N_{k-1})M_{k-1}\\ \end{cases} ⎩⎪⎨⎪⎧Nk−1=Mk−1Γk−1(Qk−1−1+Γk−1TMk−1Γk−1)−1Γk−1T（正定可逆）Mk−1=Φk/k−1−TIk−1Φk/k−1−1(非负正定的)Ik/k−1=(I−Nk−1)Mk−1 引入符号： S ^ k \hat S_k S^k，令 S ^ k = I k X ^ k \hat S_k=I_k\hat X_k S^k=IkX^k 已知： P k − 1 X ^ k = P k / k − 1 − 1 X ^ k / k − 1 + H k T R k − 1 Z k ( 推 导 略 ) P_k^{-1}\hat X_k=P_{k/k-1}^{-1}\hat X_{k/k-1}+H_k^TR_k^{-1}Z_k(推导略) Pk−1X^k=Pk/k−1−1X^k/k−1+HkTRk−1Zk(推导略) 得到： S ^ k = I k X ^ k = P k / k − 1 − 1 X ^ k / k − 1 + H k T R k − 1 Z k = S ^ k / k − 1 + H k T R k − 1 Z k S ^ k / k − 1 = I k / k − 1 X ^ k / k − 1 = ( I − N k − 1 ) Φ k / k − 1 − T S ^ k − 1 \hat S_k=I_k\hat X_k=P_{k/k-1}^{-1}\hat X_{k/k-1}+H_k^TR_k^{-1}Z_k=\hat S_{k/k-1}+H_k^TR_k^{-1}Z_k \\ \hat S_{k/k-1}=I_{k/k-1}\hat X_{k/k-1}=(I-N_{k-1})\Phi_{k/k-1}^{-T}\hat S_{k-1} S^k=IkX^k=Pk/k−1−1X^k/k−1+HkTRk−1Zk=S^k/k−1+HkTRk−1ZkS^k/k−1=Ik/k−1X^k/k−1=(I−Nk−1)Φk/k−1−TS^k−1 整理可得常用信息滤波公式为： { N k − 1 = M k − 1 Γ k − 1 ( Q k − 1 − 1 + Γ k − 1 T M k − 1 Γ k − 1 ) − 1 Γ k − 1 T （ 正 定 可 逆 ） M k − 1 = Φ k / k − 1 − T I k − 1 Φ k / k − 1 − 1 ( 非 负 正 定 的 ) I k / k − 1 = ( I − N k − 1 ) M k − 1 I k = I k / k − 1 + H k T R k − 1 H k S ^ k / k − 1 = ( I − N k − 1 ) Φ k / k − 1 − T S ^ k − 1 S ^ k = S ^ k / k − 1 + H k T R k − 1 Z k \begin{cases} N_{k-1}=M_{k-1}\Gamma_{k-1}(Q_{k-1}^{-1}+\Gamma_{k-1}^TM_{k-1}\Gamma_{k-1})^{-1}\Gamma_{k-1}^T（正定可逆）\\ M_{k-1}=\Phi^{-T}_{k/k-1}I_{k-1}\Phi^{-1}_{k/k-1}(非负正定的)\\ I_{k/k-1}=(I-N_{k-1})M_{k-1}\\ I_k=I_{k/k-1}+H_k^TR_k^{-1}H_k\\ \hat S_{k/k-1}=(I-N_{k-1})\Phi_{k/k-1}^{-T}\hat S_{k-1}\\ \hat S_k=\hat S_{k/k-1}+H_k^TR_k^{-1}Z_k \\ \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧Nk−1=Mk−1Γk−1(Qk−1−1+Γk−1TMk−1Γk−1)−1Γk−1T（正定可逆）Mk−1=Φk/k−1−TIk−1Φk/k−1−1(非负正定的)Ik/k−1=(I−Nk−1)Mk−1Ik=Ik/k−1+HkTRk−1HkS^k/k−1=(I−Nk−1)Φk/k−1−TS^k−1S^k=S^k/k−1+HkTRk−1Zk 与kalman滤波相比 优点在于初始误差矩阵 P 0 P_0 P0可以设置为无限大，当 I k = 0 I_k=0 Ik=0时表示对初始状态一无所知
假设对同一系统状态，用多个或多种传感器进行量测，可以得到： { X ^ 1 = X 1 + V 1 X ^ 2 = X 2 + V 2 . . . X ^ n = X 3 + V N s t . E [ V i ] = 0 ; E [ V i V j T ] = P i δ i j ( P i 正 定 可 逆 ) ， 角 标 表 示 不 同 数 据 来 源 \begin{cases} \hat X_1=X_1+V_1\\ \hat X_2=X_2+V_2\\ ...\\ \hat X_n=X_3+V_N \end{cases}\\ st.E[V_i]=0;E[V_iV_j^T]=P_i\delta_{ij}(P_i正定可逆)，角标表示不同数据来源 ⎩⎪⎪⎪⎨⎪⎪⎪⎧X^1=X1+V1X^2=X2+V2...X^n=X3+VNst.E[Vi]=0;E[ViVjT]=Piδij(Pi正定可逆)，角标表示不同数据来源 使用{ X 1 , X 2 , . . . , X n X_1,X_2,...,X_n X1,X2,...,Xn}对X进行联合估计且设置 I 0 = 0 I_0=0 I0=0： I N = P 1 − 1 + P 2 − 1 + P 3 − 1 + . . . + P N − 1 = ( P g ) − 1 I_N=P_1^{-1}+P_2^{-1}+P_3^{-1}+...+P_N^{-1}=(P_g)^{-1} IN=P1−1+P2−1+P3−1+...+PN−1=(Pg)−1 S ^ N = P 1 − 1 X ^ 1 + P 2 − 1 X ^ 2 + P 3 − 1 X ^ 3 + . . . + P N − 1 X ^ N = ( P g ) − 1 X ^ g \hat S_N=P_1^{-1}\hat X_1+P_2^{-1}\hat X_2+P_3^{-1}\hat X_3+...+P_N^{-1}\hat X_N=(P_g)^{-1}\hat X_g S^N=P1−1X^1+P2−1X^2+P3−1X^3+...+PN−1X^N=(Pg)−1X^g Ex：当有两种信息来源时： N = 2 N=2 N=2 当两个数据来源不相关时： P g = ( P 1 − 1 + P 2 − 1 ) − 1 = P 1 − P 1 ( P 1 + P 2 ) − 1 P 1 ( 使 用 矩 阵 求 逆 引 理 ) X ^ g = P g ( P 1 − 1 X ^ 1 + P 2 − 1 X ^ 2 ) = X ^ 1 + P 1 ( P 1 + P 2 ) − 1 ( X ^ 2 − X ^ 1 ) P_g=(P_1^{-1}+P_2^{-1})^{-1} =P_1-P_1(P_1+P_2)^{-1}P_1(使用矩阵求逆引理)\\ \hat X_g=P_g(P_1^{-1}\hat X_1+P_2^{-1}\hat X_2)=\hat X_1+P_1(P_1+P_2)^{-1}(\hat X_2-\hat X_1) Pg=(P1−1+P2−1)−1=P1−P1(P1+P2)−1P1(使用矩阵求逆引理)X^g=Pg(P1−1X^1+P2−1X^2)=X^1+P1(P1+P2)−1(X^2−X^1) 当两个数据来源相关时： P 12 = P 21 T = E [ ( X 1 − X ^ 1 ) ( X 2 − X ^ 2 ) T ] P_{12}=P_{21}^T=E[(X_1-\hat X_1)(X_2-\hat X_2)^T] P12=P21T=E[(X1−X^1)(X2−X^2)T] P g = P 1 − ( P 1 − P 12 ) ( P 1 + P 2 − P 12 − P 21 ) − 1 ( P 1 − P 21 ) P_g=P_1-(P_1-P_{12})(P_1+P_2-P_{12}-P_{21})^{-1}(P_1-P_{21}) Pg=P1−(P1−P12)(P1+P2−P12−P21)−1(P1−P21) X ^ g = X ^ 1 − ( P 1 − P 12 ) ( P 1 + P 2 − P 12 − P 21 ) − 1 ( X ^ 2 − X ^ 1 ) \hat X_g=\hat X_1-(P_1-P_{12})(P_1+P_2-P_{12}-P_{21})^{-1}(\hat X_2-\hat X_1) X^g=X^1−(P1−P12)(P1+P2−P12−P21)−1(X^2−X^1)
