信息滤波(Information Filter)的推导

信息滤波(Information Filter)的推导过程

上一篇：Kalman滤波推导

介绍

Kalman Filter的对偶形式是信息滤波器(information filter或者IF)。和EKF一样，信息滤波器也是用高斯分布表示置信度(belief)。IF和Kalman Filter的主要区别是表达高斯分布的方式有区别。

在Kalman Filter族的算法中，高斯函数采用它们的矩来表示（均值和协方差）。而Information Filter采用正则参数（信息矩阵和信息向量）表示高斯函数。这两种表示形式互为对偶。

正则参数Canonical Parameterization

多维高斯的正则参数表示为一个矩阵 $I$(不是单位矩阵，本文单位矩阵是$\mathcal{I}$)和一个向量 $\hat{\xi }$。信息矩阵 $I$是协方差矩阵的逆：
$$I={\Sigma }^{-1}$$
信息向量$\hat{\xi }$用矩表示为：
$$\hat{\xi }={\Sigma }^{-1}\mu$$
易知 $I$和$\hat{\xi }$是高斯概率分布的完备参数。由正则参数推导矩参数的方程如下：
$$\begin{array}{rl}\Sigma & =I^{-1}\\ \mu & =I^{-1}\hat{\xi }\end{array}$$
多维高斯概率分布函数：
$$\begin{array}{rl}P\left(x\right)& =\det {\left(2\pi \Sigma \right)}^{-\frac{1}{2}}\exp \left\{-\frac{1}{2}{\left(x-\mu \right)}^T{\Sigma }^{-1}\left(x-\mu \right)\right\}\\ & =\det {\left(2\pi \Sigma \right)}^{-\frac{1}{2}}\exp \left\{-\frac{1}{2}{x}^T{\Sigma }^{-1}x+x^T{\Sigma }^{-1}\mu -\frac{1}{2}{\mu }^T{\Sigma }^{-1}\mu \right\}\\ & =\underset{\text{const}}{\underbrace{\det {\left(2\pi \Sigma \right)}^{-\frac{1}{2}}\exp \left\{-\frac{1}{2}{\mu }^T{\Sigma }^{-1}\mu \right\}}}\exp \left\{-\frac{1}{2}{x}^T{\Sigma }^{-1}x+x^T{\Sigma }^{-1}\mu \right\}\end{array}$$
标为$\text{const}$的部分和 $x$无关，是一个常数。

注意：高斯概率分布函数中$\Sigma$是协方差，$\mu$是期望；而在Kalman滤波器中$P_k$是协方差，${\hat{x}}_k$类似于期望。

信息滤波的推导过程

和Kalman滤波器一样，系统的状态转移方程和测量方程如下：
$$\begin{array}{rl}{x}_k& =A_k{x}_{k-1}+B_k{u}_k+C_k{w}_k\\ y_k& =H_k{x}_k+v_k\end{array}$$
因为信息滤波器是Kalman滤波器的对偶，我们可以从Kalman方程对应到信息滤波器方程。Kalman滤波器方程：

Predict
$$\begin{array}{rlr}{\hat{x}}_{k/k-1}& =A_k{\hat{x}}_{k-1}+B_k{u}_k& 状态一步预测(先验)\\ P_{k/k-1}& =A_k{P}_{k-1}{A}_k^T+C_k{Q}_k{C}_k^T& 状态一步预测均方误差阵(先验)\end{array}$$
Update
$$\begin{array}{rlr}{K}_k& =P_{k/k-1}{H}_k^T{\left(H_k{P}_{k/k-1}{H}_k^T+R_k\right)}^{-1}& 滤波增益\\ {\hat{x}}_k& ={\hat{x}}_{k/k-1}+K_k\left(y_k-H_k{\hat{x}}_{k/k-1}\right)& 状态估计(后验)\\ P_k& =\left(\mathcal{I}-K_k{H}_k\right)P_{k/k-1}& 状态估计均方误差阵(后验)\end{array}$$
在Kalman滤波中，状态估计的均方误差阵定义为$P_k=E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T]$，如果估计值${\hat{x}}_k$接近真实值$x_k$，则$P_k$应当很小，对应于$P_k^{-1}\to \infty$；反之，如果${\hat{x}}_k$远离真实值，则$P_k$应当很大,对应于$P_k^{-1}\to O$。这说明，$P_k^{-1}$反映了估计值${\hat{x}}_k$与真实值$x_k$的接近程度，$P_k^{-1}$越大则越接近，反之亦然。因此，可以将$P_k^{-1}$看作是衡量估计值${\hat{x}}_k$中含有真实状态$x_k$信息量多少的指标，$P_k^{-1}$越大则信息越多，即状态估计越准确，习惯上称$P_k^{-1}$为信息矩阵，可重记为$I_k=P_k^{-1}$。这就是信息滤波的由来。简单来说，信息滤波就是将常规kalman式子全部用$I_k$表示。

信息滤波的推导过程如下：

根据正则参数和矩参数的关系，
$$\begin{array}{rl}I& ={\Sigma }^{-1}\\ \hat{\xi }& ={\Sigma }^{-1}\mu \end{array}$$
可知
$$\begin{gathered} I_k=P_k^{-1} \\ {\hat{\xi }}_k=P_k^{-1}{\hat{x}}_k \end{gathered}$$
对于信息滤波器，预测信息矩阵的方法如下：
$$\begin{array}{rl}{I}_{k/k-1}^{-1}& =A_k{I}_{k-1}^{-1}{A}_k^T+C_k{Q}_k{C}_k^T\\ \Rightarrow I_{k/k-1}& ={\left(A_k{I}_{k-1}^{-1}{A}_k^T+C_k{Q}_k{C}_k^T\right)}^{-1}\end{array}$$
预测信息向量的方法如下：
$$\begin{array}{rl}{\hat{x}}_{k/k-1}& =I_{k/k-1}^{-1}{\hat{\xi }}_{k/k-1}\\ {\hat{x}}_{k-1}& =I_{k-1}^{-1}{\hat{\xi }}_{k-1}\\ {\hat{x}}_{k/k-1}& =A_k{\hat{x}}_{k-1}+B_k{u}_k\\ \Rightarrow I_{k/k-1}^{-1}{\hat{\xi }}_{k/k-1}& =A_k{I}_{k-1}^{-1}{\hat{\xi }}_{k-1}+B_k{u}_k\\ \Rightarrow {\hat{\xi }}_{k/k-1}& =I_{k/k-1}\left(A_k{I}_{k-1}^{-1}{\hat{\xi }}_{k-1}+B_k{u}_k\right)\end{array}$$
下面来计算如何用正则参数表达 $K_k$。在计算用正则参数表达$K_k$时，需要用到Matrix Inverse Lemma
$${\left(A+BCD\right)}^{-1}=A^{-1}-A^{-1}B{\left(C^{-1}+D{A}^{-1}B\right)}^{-1}D{A}^{-1}$$
对于$K_k=P_{k/k-1}{H}_k^T{\left(H_k{P}_{k/k-1}{H}_k^T+R_k\right)}^{-1}$中的${\left(H_k{P}_{k/k-1}{H}_k^T+R_k\right)}^{-1}$，我们可以用Matrix Inverse Lemma转化为：
$${\left(H_k{P}_{k/k-1}{H}_k^T+R_k\right)}^{-1}=R_k^{-1}-R_k^{-1}{H}_k{\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{R}_k^{-1}$$
那么 $K_k$
$$\begin{array}{rl}{K}_k& =P_{k/k-1}{H}_k^T{\left(H_k{P}_{k/k-1}{H}_k^T+R_k\right)}^{-1}\\ & =P_{k/k-1}{H}_k^T{R}_k^{-1}-P_{k/k-1}{H}_k^T{R}_k^{-1}{H}_k{\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{R}_k^{-1}\\ & =P_{k/k-1}\left[\mathcal{I}-H_k^T{R}_k^{-1}{H}_k{\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}\right]H_k^T{R}_k^{-1}\\ & =P_{k/k-1}\left[\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right){\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}-H_k^T{R}_k^{-1}{H}_k{\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}\right]H_k^T{R}_k^{-1}\\ & ={\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{R}_k^{-1}\end{array}$$
可以进一步计算更新过程中的信息矩阵：
$$\begin{array}{rl}{I}_k^{-1}& =P_k=\left(\mathcal{I}-K_k{H}_k\right)P_{k/k-1}\\ & =\left(\mathcal{I}-{\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{R}_k^{-1}{H}_k\right)P_{k/k-1}\\ & ={\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}\\ & ={\left(I_{k/k-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}\\ \Rightarrow I_k& =I_{k/k-1}+H_k^T{R}_k^{-1}{H}_k\end{array}$$
所以$K_k$还有另一种简单的形式如下：
$$\begin{array}{rl}{K}_k& ={\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{R}_k^{-1}\\ & =I_k^{-1}{H}_k^T{R}_k^{-1}\end{array}$$
现在来计算更新过程中的信息向量：
$$\begin{array}{rl}{I}_k^{-1}{\hat{\xi }}_k& ={\hat{x}}_k={\hat{x}}_{k/k-1}+K_k\left(y_k-H_k{\hat{x}}_{k/k-1}\right)=\left(\mathcal{I}-K_k{H}_k\right){\hat{x}}_{k/k-1}+K_k{y}_k\\ & ={\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}{P}_{k/k-1}^{-1}{\hat{x}}_{k/k-1}+K_k{y}_k\\ \Rightarrow {\hat{\xi }}_k& =I_k{\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}{P}_{k/k-1}^{-1}{\hat{x}}_{k/k-1}+I_k{K}_k{y}_k\\ & =P_{k/k-1}^{-1}{\hat{x}}_{k/k-1}+I_k{K}_k{y}_k\\ & ={\hat{\xi }}_{k/k-1}+I_k{\left(P_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{R}_k^{-1}{y}_k\\ & =H_k^T{R}_k^{-1}{y}_k+{\hat{\xi }}_{k/k-1}\end{array}$$
至此，我们已经全部推导出信息滤波器的公式。

(1) 用信息矩阵和信息向量递推

Predict
$$\begin{array}{rlr}{I}_{k/k-1}& ={\left(A_k{I}_{k-1}^{-1}{A}_k^T+C_k{Q}_k{C}_k^T\right)}^{-1}& 一步预测信息矩阵(先验)\\ {\hat{\xi }}_{k/k-1}& =I_{k/k-1}\left(A_k{I}_{k-1}^{-1}{\hat{\xi }}_{k-1}+B_k{u}_k\right)& 一步预测信息向量(先验)\end{array}$$
Update
$$\begin{array}{rlr}{I}_k& =I_{k/k-1}+H_k^T{R}_k^{-1}{H}_k& 信息矩阵(后验)\\ {\hat{\xi }}_k& =H_k^T{R}_k^{-1}{y}_k+{\hat{\xi }}_{k/k-1}& 信息向量估计(后验)\end{array}$$

仿真：(模型和Kalman的一样)

clear;clc;close all;
N=11;% 采样点数
n=2;% 本系统2维
%% 状态和测量值初始化
X=zeros(n,N);%各时刻真实值,初始值为0
X_kf=zeros(n,N);% 估计值(Kalman滤波处理的状态)
Y=zeros(2,N); %测量值
K=zeros(n,2,N); 
I=eye(n); 
%% 噪声
Q=1;%W(k)的方差
R1=1;%V1(k)的方差
R2=2;%V2(k)的方差
R=[R1 0;0 R2];
rng(4);%确定随机种子，防止每次W和V不同
W=sqrt(Q)*randn(1,N);
V1=sqrt(R1)*randn(1,N);
V2=sqrt(R2)*randn(1,N);
V=[V1;V2];
%% 赋初值
X(:,1)=[0;0];%系统初值
P(:,:,1)=I;%初始值的协方差

sigma_x1_fang(1)=P(1,1,1);
sigma_x2_fang(1)=P(2,2,1);
X_kf(:,1)=[0.5;0.2];%滤波器的初始估计状态，不知道，任意
%% 系统矩阵
A=[1 1;0 1];
B=[0;0];
u_k=0;
C=[1;1];
H=[1 0;0 1];
%% 将迭代使用的参数进行变换
I(:,:,1)=inv(P(:,:,1)); % 信息矩阵
Xtrans_kf(:,1)=I(:,:,1)*X_kf(:,1);  % 将状态变换后的后验值
%% 模拟测量过程,并滤波
for k=2:N
    X(:,k)=A*X(:,k-1)+B*u_k+C*W(k-1);  
    Y(:,k)=H*X(:,k)+V(:,k);

    P_pre=A*inv(I(:,:,k-1))*A'+C*Q*C'; 
    I_pre=inv(P_pre);   % 先验信息矩阵
    Xtrans_pre=I_pre*(A*inv(I(:,:,k-1))*Xtrans_kf(:,k-1)+B*u_k);


    I(:,:,k)=I_pre+H'*inv(R)*H;% 后验信息矩阵
    % K(:,:,k)=inv(I(:,:,k))*H'*inv(R); 

    Xtrans_kf(:,k)=Xtrans_pre+H'*inv(R)*Y(:,k);
    
    P(:,:,k)=inv(I(:,:,k));% 这个P仅仅是绘图用的
    sigma_x1_fang(k)=P(1,1,k);
    sigma_x2_fang(k)=P(2,2,k);

    X_kf(:,k)=inv(I(:,:,k))*Xtrans_kf(:,k);% 这个X_kf仅仅是绘图用的
end
%% 误差计算
Err_Kalman_x1=X(1,:)-X_kf(1,:);
Err_Kalman_x2=X(2,:)-X_kf(2,:);
%% 绘图
t=0:N-1;
figure('Name','信息滤波仿真')
plot(t,X_kf(1,:),'-.b','LineWidth',1.5);hold on 
plot(t,X(1,:),'-ob','LineWidth',1.5);hold on 
plot(t,X_kf(2,:),'-.k','LineWidth',1.5);hold on 
plot(t,X(2,:),'-ok','LineWidth',1.5);
legend('kalman估计值x_1','真实值x_1','kalman估计值x_2','真实值x_2');         
xlabel('采样时间');
ylabel('kalman估计值和真实值');
title('kalman估计值和真实值');

figure('Name','误差分析')
plot(t,Err_Kalman_x1,'-^b','LineWidth',1.5);
hold on 
plot(t,Err_Kalman_x2,'-^k','LineWidth',1.5);
legend('x_1误差','x_2误差');         
xlabel('采样时间');
ylabel('误差');
title('误差');

figure('Name','误差方差分析')
plot(t,sigma_x1_fang,'-*b','LineWidth',1.5);
hold on 
plot(t,sigma_x2_fang,'-*k','LineWidth',1.5);
legend('x_1误差的方差','x_2误差的方差');         
xlabel('采样时间');
ylabel('误差的方差');
title('状态估计误差的方差');

figure('Name','误差图')
sigma_x1=sigma_x1_fang.^0.5;
sigma_x2=sigma_x2_fang.^0.5;
errorbar(t, X_kf(1,:),3.*sigma_x1,'-ob','LineWidth',1.5);hold on 
plot(t,X(1,:),'-.b','LineWidth',1.5);hold on 
errorbar(t, X_kf(2,:),3.*sigma_x2,'-ok','LineWidth',1.5);hold on 
plot(t,X(2,:),'-.k','LineWidth',1.5);hold on 
legend('kalman估计值x_1误差线','真实值x_1','kalman估计值x_2误差线','真实值x_2');  
xlabel('采样时间');
ylabel('kalman估计值误差线和真实值');

仿真结果：

(2) 用信息矩阵和Kalman估计值递推

信息矩阵的先验和后验与上面(1)中的一样，如下所示：
$$\begin{array}{rlr}{I}_{k/k-1}& ={\left(A_k{I}_{k-1}^{-1}{A}_k^T+C_k{Q}_k{C}_k^T\right)}^{-1}& 一步预测信息矩阵(先验)\\ I_k& =I_{k/k-1}+H_k^T{R}_k^{-1}{H}_k& 信息矩阵(后验)\end{array}$$
增益就是上面推导的，如下所示：
$$K_k=I_k^{-1}{H}_k^T{R}_k^{-1}滤波增益$$
Kalman估计值和经典的Kalman滤波一样，如下所示：
$$\begin{array}{rlr}{\hat{x}}_{k/k-1}& =A_k{\hat{x}}_{k-1}+B_k{u}_k& 状态一步预测\\ {\hat{x}}_k& ={\hat{x}}_{k/k-1}+K_k\left(y_k-H_k{\hat{x}}_{k/k-1}\right)& 状态估计\end{array}$$

仿真：(模型和Kalman的一样)

clear;clc;close all;
N=11;% 采样点数
n=2;% 本系统2维
%% 状态和测量值初始化
X=zeros(n,N);%各时刻真实值,初始值为0
X_kf=zeros(n,N);% 估计值(Kalman滤波处理的状态)
Y=zeros(2,N); %测量值
K=zeros(n,2,N); 
I=eye(n); 
%% 噪声
Q=1;%W(k)的方差
R1=1;%V1(k)的方差
R2=2;%V2(k)的方差
R=[R1 0;0 R2];
rng(4);%确定随机种子，防止每次W和V不同
W=sqrt(Q)*randn(1,N);
V1=sqrt(R1)*randn(1,N);
V2=sqrt(R2)*randn(1,N);
V=[V1;V2];
%% 赋初值
X(:,1)=[0;0];%系统初值
P(:,:,1)=I;%初始值的协方差

sigma_x1_fang(1)=P(1,1,1);
sigma_x2_fang(1)=P(2,2,1);
X_kf(:,1)=[0.5;0.2];%滤波器的初始估计状态，不知道，任意
%% 系统矩阵
A=[1 1;0 1];
B=[0;0];
u_k=0;
C=[1;1];
H=[1 0;0 1];
%% 模拟测量过程,并滤波
for k=2:N
    X(:,k)=A*X(:,k-1)+B*u_k+C*W(k-1);  
    Y(:,k)=H*X(:,k)+V(:,k);

    X_pre=A*X_kf(:,k-1)+B*u_k;
    P_pre=A*inv(I(:,:,k-1))*A'+C*Q*C'; 
    I_pre=inv(P_pre);   % 先验信息矩阵

    I(:,:,k)=I_pre+H'*inv(R)*H;% 后验信息矩阵
    K(:,:,k)=inv(I(:,:,k))*H'*inv(R); 

    X_kf(:,k)=X_pre+K(:,:,k)*(Y(:,k)-H*X_pre);
    
    P(:,:,k)=inv(I(:,:,k));% 这个P仅仅是绘图用的
    sigma_x1_fang(k)=P(1,1,k);
    sigma_x2_fang(k)=P(2,2,k);
end
%% 误差计算
Err_Kalman_x1=X(1,:)-X_kf(1,:);
Err_Kalman_x2=X(2,:)-X_kf(2,:);
%% 绘图
t=0:N-1;
figure('Name','信息滤波仿真')
plot(t,X_kf(1,:),'-.b','LineWidth',1.5);hold on 
plot(t,X(1,:),'-ob','LineWidth',1.5);hold on 
plot(t,X_kf(2,:),'-.k','LineWidth',1.5);hold on 
plot(t,X(2,:),'-ok','LineWidth',1.5);
legend('kalman估计值x_1','真实值x_1','kalman估计值x_2','真实值x_2');         
xlabel('采样时间');
ylabel('kalman估计值和真实值');
title('kalman估计值和真实值');

figure('Name','误差分析')
plot(t,Err_Kalman_x1,'-^b','LineWidth',1.5);
hold on 
plot(t,Err_Kalman_x2,'-^k','LineWidth',1.5);
legend('x_1误差','x_2误差');         
xlabel('采样时间');
ylabel('误差');
title('误差');

figure('Name','误差方差分析')
plot(t,sigma_x1_fang,'-*b','LineWidth',1.5);
hold on 
plot(t,sigma_x2_fang,'-*k','LineWidth',1.5);
legend('x_1误差的方差','x_2误差的方差');         
xlabel('采样时间');
ylabel('误差的方差');
title('状态估计误差的方差');

figure('Name','误差图')
sigma_x1=sigma_x1_fang.^0.5;
sigma_x2=sigma_x2_fang.^0.5;
errorbar(t, X_kf(1,:),3.*sigma_x1,'-ob','LineWidth',1.5);hold on 
plot(t,X(1,:),'-.b','LineWidth',1.5);hold on 
errorbar(t, X_kf(2,:),3.*sigma_x2,'-ok','LineWidth',1.5);hold on 
plot(t,X(2,:),'-.k','LineWidth',1.5);hold on 
legend('kalman估计值x_1误差线','真实值x_1','kalman估计值x_2误差线','真实值x_2');  
xlabel('采样时间');
ylabel('kalman估计值误差线和真实值');

仿真结果：

结论：三者仿真一模一样，所以说IF和Kalman滤波本质一样，形式互为对偶。

为了避免$I_{k/k-1},I_k$存在不可逆的情况，利用矩阵求逆引理运算，进行如下转换：
$$\begin{array}{rl}{I}_{k/k-1}& ={\left(A_k{I}_{k-1}^{-1}{A}_k^T+C_k{Q}_k{C}_k^T\right)}^{-1}\\ & =[\mathcal{I}-M_{k-1}{C}_k(Q_k^{-1}+C_k^T{M}_{k-1}{C}_k{)}^{-1}{C}_k^T]A_k^{-T}{I}_{k-1}{A}_k^{-1}\\ & =(\mathcal{I}-N_{k-1})M_{k-1}\end{array}$$
其中
$$\{\begin{array}{ll}{N}_{k-1}=M_{k-1}{C}_k(Q_k^{-1}+C_k^T{M}_{k-1}{C}_k{)}^{-1}{C}_k^T& (正定可逆)\\ M_{k-1}=A_k^{-T}{I}_{k-1}{A}_k^{-1}& (非负正定)\end{array}$$