多速率多传感器数据融合估计（二）

最新推荐文章于 2022-03-01 10:33:07 发布

咸鱼.m

最新推荐文章于 2022-03-01 10:33:07 发布

阅读量1.5k

点赞数 3

分类专栏：卡尔曼滤波算法文章标签：线性代数矩阵概率论算法

本文链接：https://blog.csdn.net/qq_37207042/article/details/104480180

版权

卡尔曼滤波算法专栏收录该内容

5 篇文章 19 订阅

订阅专栏

问题描述

首先单模型多速率同步动态模型已知。
在这里插入图片描述
对上述模型进行改写可以得到

由于是分布式滤波所以每个传感器独立进行滤波，其实上篇文章中的状态分块滤波个人认为也属于分布式滤波，只不过是滤波并不在最细尺度上进行，是根据融合周期M的整个数据块进行滤波，每个传感器的滤波也是相对独立的，在融合周期的时间点上进行融合得到最优估计。相对于本章的分布式滤波，状态分块的实时性较差。
噪声的白色性kalman滤波的基本知识不再介绍（上篇没有提及，各位知道就好）。
在这里插入图片描述
这里的 $A_i$ 进行了重构，这个式子很明显由于传感器 $\ i$ 的采样间隔为 $n_i$ 所以将采样间隔中每个单位时间的状态转移矩阵累乘，得到重构的 $A_i$ 。

Remark:传感器1为最细尺度上的滤波，所以传感器1的参数与原始参数（未重构）一致。传感器随序号的增加采样间隔越大（惯例）。

参数的推导

在这里插入图片描述
这里与上篇推导状态转移方程相仿，只不过是代入方向相反，这个是将状态转移方程右侧代入，时间向前的代入方式。将式子右侧的 $x(n_ik+n_i)$ 通过不断的用
$\ x(n_ik+n_i-j)=A(n_ik+n_i-j-1)x(n_ik+n_i-j-1)+G(n_ik+n_i-j-1)u(n_ik+n_i-j-1)+\Gamma(n_ik+n_i-j-1)w(n_ik+n_i-j-1)$ 代入
上述式子可以写的简便（此处为了上述推导容易理解采用了上述式子从的符号表示）
通过将推导中的累加写成矩阵相乘的形式，可以得到重构的 $\ A_i.G_i,\Gamma_i,u_i,w_i$ 这些在问题描述中已经给出表达式。

分布式无反馈数据融合算法

分布式顾名思义，传感器之间的滤波是互不干扰。无反馈的含义为，融合得到的结果并不反馈到每个传感器，融合结果对后续的滤波没有影响。

融合算法

分两种情况，第一种情况：当前时刻只有传感器1（最细尺度）存在采样数据，此时传感器1自身进行标准kalman滤波，得到的结果即为当前时刻的最优估计（如果是状态分块算法，这个时刻是不存在滤波结果的所以具有时延性）。
第二种情况：当前时刻其他传感器（传感器2，3……N）有采样数据，此时对有采样数据的传感器分别进行标准kalman滤波，分别得到滤波结果进行融合。融合算法如下：
在这里插入图片描述

$\alpha的物理意义就是代表传感器i融合的权重，其中i_p代表的是当前时刻有采样数据的传感器（非传感器1），\hat{x}_f与P_f代表的是多个传感器融合后的最优估计$ 。得到融合后的结果后并不反馈给每个传感器进行下一步的预测，这是无反馈数据融合。

分布式有反馈数据融合算法

融合算法：
分为三种情况：
第一种只有传感器1（最细尺度）存在采样观测数据。则传感器1自身的滤波估计即为全局的滤波估计最优。
在这里插入图片描述

上述式子通俗的进行描述的话，第一个式子中 $\hat{x}_1(k|k-1)$ 代表的是预测值， $K_1$ 代表kalman增益，可以理解为一个权，后面的方括号中即为预测值与观测值的偏差，那么我们如何利用这个偏差修正我预测值得到最优估计，关键在于kalman增益，方括号中 $z_1(k)$ 为观测， $\ C_1\hat{x}(k|k-1)$ 为预测，为什么乘以 $C_1$ 因为观测量可能不是全维观测，或者存在放缩，所以需要观测矩阵 $C_1$ 。 $y_1(k)$ 代表的是观测量固定的偏差，类似与误差一样的，这个误差我们已知，所以这里减去可以消除偏差（也就是消除观测固定误差，随机误差无法消除）。
第二个式子，右侧第一项即为状态转移，第二项为外加作用（外部控制）。
第三个式子协方差矩阵右侧第一项为状态转移后的协方差更新，第二项为状态转移方程存在的过程噪声，引起的协方差变化，等式左侧的协方差矩阵代表着预测值的确信度。
第四个式子，等式右侧将预测值的确信度与观测值的确信度之和作为分母，预测值的确信度作为分子，得到了kalman增益。
最后一个式子用来计算融合后的结果的确信度，如果状态为一维的kalman增益必然是小于1的值，这里因为进行了修正所以用单位矩阵减去kalman增益再与协方差矩阵相乘，这时结果的协方差矩阵会“变小”，也可以将协方差理解为噪声。噪声大随机误差大，反之同理。
第二种情况：对于传感器 $\ i(2<=i<=N)$ 在时刻 $\ k$ 存在观测（注意这里指的是传感器1与另外一个传感器存在观测），记 $\ l=\frac{k}{n_i}$ 其中 $n_i$ 为传感器 $\ i$ 的采样间隔。
融合算法：
在这里插入图片描述
传感器 $\ i$ 的滤波估计算法：

解释同上面的一样。
加权矩阵为：

**第三种情况：**存在多个传感器同时在时刻 $\ k$ 有观测数据，存在 $i_i,i_2……,i_j(2<=i_1,i_2……,i_j<=N)$ 传感器在时刻 $\ k$ 有观测。
融合算法：
在这里插入图片描述
其中 $l_p=\frac{k}{n_{i_p}},p=1,2,……，j$
下面给出对于传感器 $i_p$ 的滤波算法:
滤波算法都与上述两种情况类似，重点在于融合算法与加权矩阵的计算。
加权矩阵为：

此时对于算法有了清楚的了解，但是对于算法流程不太清楚，有反馈与无反馈有何不同，这里给出流程图
在这里插入图片描述
此图描述的是第三种情况下的融合，第二种情况也类似，算法内容进行调整，流程一致，第一种情况则抛出下半部分即可。有反馈从图中可以看出在中部右侧 $\hat{x}(k|k)$ 的前面延迟 $n_{i_p}$ 步后用于下一步的预测，此处体现了有反馈，可以看出对于传感器1并没有反馈，仅仅对于传感器 $i_p$ 进行了反馈。无反馈则是全都不反馈。

仿真实例

/无反馈数据融合
function [Xf1] = method2(A,P0,P3,Qk,C,Rk,n1,n2,M,N,v1,v2)
%UNTITLED2 此处显示有关此函数的摘要
%   此处显示详细说明
M1=M/n1;
M2=M/n2;
A1=A^n1;
A2=A^n2;

%% 滤波
X1=zeros(1,N);
X2=zeros(1,N/n2);
x1=[1];
x2=[5];
X1(1,1)=x1;
X2(1,1)=x2;
Xf1=zeros(1,N);
for k=1:N-1
    if k>2
        if rem(k,n2)==0
        %传感器1
        temp1= A1*X1(:,k);                %状态推测的转移
        P1=A1*P0*A1'+Qk;               %对状态不确定新规定转移
        Kk1 = P1*C'/(C*P1*C'+Rk);         %卡尔曼系数k的确定
        X1(:,k+1) = temp1+Kk1*(v1(:,k+1)-C*temp1);     %对下一阶段进行推测
        P0= (eye(1,1)-Kk1*C)*P1;                %不确定阵的更新,P0，P1不停的迭代
        Xf1(k+1)=X1(k+1);
    
        else
        %传感器1
        temp1= A1*X1(:,k);                %状态推测的转移
        P1 = A1*P0*A1'+Qk;               %对状态不确定新规定转移
        Kk1 = P1*C'/(C*P1*C'+Rk);         %卡尔曼系数k的确定
        X1(:,k+1) = temp1+Kk1*(v1(:,k+1)-C*temp1);     %对下一阶段进行推测
        P0= (eye(1,1)-Kk1*C)*P1;                %不确定阵的更新,P0，P1不停的迭代
       
        %传感器2
        temp2= A2*X2(:,floor(k/n2));                %状态推测的转移
        P2= A2*P3*A2'+Qk;               %对状态不确定新规定转移
        Kk2 = P2*C'/(C*P2*C'+Rk);         %卡尔曼系数k的确定
        X2(:,floor(k/n2)+1) = temp2+Kk2*(v2(:,floor(k/n2)+1)-C*temp2);     %对下一阶段进行推测
        P3= (eye(1,1)-Kk2*C)*P2;                %不确定阵的更新,不停的迭代
        %融合
         a1=inv(inv(P0)+inv(P3))*inv(P0);
         a2=inv(inv(P0)+inv(P3))*inv(P3);
         Xf1(k+1)=a1*X1(k+1)+a2*X2(floor(k/n2)+1);
        end
    elseif k==1
    temp1= A1*X1(:,k);                %状态推测的转移
    P1 = A1*P0*A1'+Qk;               %对状态不确定新规定转移
    Kk1 = P1*C'/(C*P1*C'+Rk);         %卡尔曼系数k的确定
    X1(:,k+1) = temp1+Kk1*(v1(:,k+1)-C*temp1);     %对下一阶段进行推测
    P0= (eye(1,1)-Kk1*C)*P1;                %不确定阵的更新,P0，P1不停的迭代
    Xf1(k)=X1(k);
    %融合
    a1=inv(inv(P0)+inv(P3))*inv(P0);
    a2=inv(inv(P0)+inv(P3))*inv(P3);
    Xf1(k+1)=a1*X1(k+1)+a2*X2(k);
    
    elseif k==2
    %传感器1
    temp1= A1*X1(:,k);                %状态推测的转移
    P1=A1*P0*A1'+Qk;               %对状态不确定新规定转移
    Kk1 = P1*C'/(C*P1*C'+Rk);         %卡尔曼系数k的确定
    X1(:,k+1) = temp1+Kk1*(v1(:,k+1)-C*temp1);     %对下一阶段进行推测
    P0= (eye(1,1)-Kk1*C)*P1;                %不确定阵的更新,P0，P1不停的迭代
    Xf1(k+1)=X1(k+1);
    end
end

/有反馈数据融合
function [Xf2] = method3(A,P0,P3,Qk,C,Rk,n1,n2,M,N,v1,v2)
%UNTITLED3 此处显示有关此函数的摘要
%   此处显示详细说明
M1=M/n1;
M2=M/n2;
A1=A^n1;
A2=A^n2;
%% 滤波            %修改
X1=zeros(1,N);
X2=zeros(1,N/n2);
x1=[1];
x2=[5];
X1(1,1)=x1;
X2(1,1)=x2;
Xf2=zeros(1,N);
for k=1:N-1
    if k>2
        if rem(k,n2)==0
        %传感器1
        temp1= A1*X1(:,k);                %状态推测的转移
        P1=A1*P0*A1'+Qk;               %对状态不确定新规定转移
        Kk1 = P1*C'/(C*P1*C'+Rk);         %卡尔曼系数k的确定
        X1(:,k+1) = temp1+Kk1*(v1(:,k+1)-C*temp1);     %对下一阶段进行推测
        P0= (eye(1,1)-Kk1*C)*P1;                %不确定阵的更新,P0，P1不停的迭代
        Xf2(k+1)=X1(k+1);
    
        else
        %传感器1
        temp1= A1*X1(:,k);                %状态推测的转移
        P1 = A1*P0*A1'+Qk;               %对状态不确定新规定转移
        Kk1 = P1*C'/(C*P1*C'+Rk);         %卡尔曼系数k的确定
        X1(:,k+1) = temp1+Kk1*(v1(:,k+1)-C*temp1);     %对下一阶段进行推测
        P0= (eye(1,1)-Kk1*C)*P1;                %不确定阵的更新,P0，P1不停的迭代
       
        %传感器2
        temp2= A2*Xf2(:,k-1);            %状态推测的转移
        P2= A2*Pf*A2'+Qk;               %对状态不确定新规定转移
        Kk2 = P2*C'/(C*P2*C'+Rk);       %卡尔曼系数k的确定
        X2(:,floor(k/n2)+1) = temp2+Kk2*(v2(:,floor(k/n2)+1)-C*temp2);     %对下一阶段进行推测
        P3= (eye(1,1)-Kk2*C)*P2;                %不确定阵的更新,不停的迭代
        %融合
         a1=inv(inv(P0)+inv(P3))*inv(P0);
         a2=inv(inv(P0)+inv(P3))*inv(P3);
         Xf2(k+1)=a1*X1(k+1)+a2*X2(floor(k/n2)+1);
         Pf=inv(inv(P0)+inv(P3));
        end
    elseif k==1
    temp1= A1*X1(:,k);                %状态推测的转移
    P1 = A1*P0*A1'+Qk;               %对状态不确定新规定转移
    Kk1 = P1*C'/(C*P1*C'+Rk);         %卡尔曼系数k的确定
    X1(:,k+1) = temp1+Kk1*(v1(:,k+1)-C*temp1);     %对下一阶段进行推测
    P0= (eye(1,1)-Kk1*C)*P1;                %不确定阵的更新,P0，P1不停的迭代
    Xf2(k)=X1(k);
    %融合
    a1=inv(inv(P0)+inv(P3))*inv(P0);
    a2=inv(inv(P0)+inv(P3))*inv(P3);
    Xf2(k+1)=a1*X1(k+1)+a2*X2(k);
    Pf=inv(inv(P0)+inv(P3));
    elseif k==2
    %传感器1
    temp1= A1*X1(:,k);                %状态推测的转移
    P1=A1*P0*A1'+Qk;               %对状态不确定新规定转移
    Kk1 = P1*C'/(C*P1*C'+Rk);         %卡尔曼系数k的确定
    X1(:,k+1) = temp1+Kk1*(v1(:,k+1)-C*temp1);     %对下一阶段进行推测
    P0= (eye(1,1)-Kk1*C)*P1;                %不确定阵的更新,P0，P1不停的迭代
    Xf2(k+1)=X1(k+1);
    end
end

/主函数
%------三种多速率卡尔曼滤波的比较-----------%
clc;
clear all;  
close all; 
%% 观测数据生成
N=100; %时间跨度
n1=1;%采样间隔
n2=2;
M=lcm(n1,n2);%采样间隔的最小公倍数  即为融合周期
M1=M/n1;
M2=M/n2;
t=1:1:N ;  %时间序列
a=2 ; %加速度  
v=a*t;   %标准速度轨迹
wt = randn(1,N);  
wt = sqrt(9)*wt;%方差为9的正态分布噪声
v1=v+wt;
wt = randn(1,N);  
wt = sqrt(4)*wt;%方差为4的正态分布噪声
v2=v+wt;
%v1  v2  加入的噪声方差不同
%对v2进行提取保持采样率为v1的一半
v3=zeros(1,N/2);
for i=1:1:N/2
    v3(1,i)=v2(1,i*n2);
end
v2=v3;
%此时v1采样间隔为1，v2采样间隔为2
Z1=cell(1,N/M);
Z2=cell(1,N/M);
for i=1:50
   Z1(i)={[v1(2*i-1),v1(2*i)]};
end
for i=1:50
    Z2(i)={v2(i)};
end

%% 调用第一种
A1=[1 0
    0 1];
C1=[1 0];
C2=[0 1];
P0=[4 2
    2 4]; %
Qk=[2 1
    1 2];
R1=[2 1 
    1 2];
R2=[1];
X0=[1 0 5 4]';
[k_v1,k_v2]=method1(A1,C1,C2,P0,Qk,R1,R2,X0,n1,n2,M,N,Z1,Z2);
%% 调用第二种
%% A1阵与A2的建立
A=[1];
%% P矩阵
P0=[4];%传感器1
P3=[4];%传感器2 
%% Q矩阵
Qk=[4];
%% C矩阵
C=[1];
%% R矩阵
Rk=[4];
[Xf1]=method2(A,P0,P3,Qk,C,Rk,n1,n2,M,N,v1,v2);
%% 调用第三种
[Xf2]=method3(A,P0,P3,Qk,C,Rk,n1,n2,M,N,v1,v2);
%% 性能观测
plot(t,v(t),'r',t,k_v1(t),'k',t,Xf1(t),'g',t,Xf2(t),'-');  
title('速度');  
legend('实际值','估计值1','估计值2','估计值3');  
xlabel('t');  
ylabel('v');  
figure(2)
plot(t,(v(t)-k_v1(t))./v(t),'k',t,(v(t)-Xf1(t))./v(t),'g',t,(v(t)-Xf2(t))./v(t),'-')
title('速度误差');  
legend('误差1','误差2','误差3');  
xlabel('t');  
ylabel('δ');