条件
设定的条件为两个传感器，同时观测一个状态量，对两个传感器对应的局部滤波进行融合（使用联邦滤波算法），并加入单独的局部滤波的对比，联邦滤波算法核心：
代码
%Project: 相关 双通道 一维 kalman滤波
%Author: Jace
%Data: 2021/09/13
%--------------------准备---------------------
close all;
clear all;
clc;
%------------------初始化参数---------------------
N=50;%设定采样点数，即持续时长
A=1.01;
H1=1; %第1个传感器量测矩阵
H2=1.2; %第2个传感器量测矩阵
Q=0.01;%状态方程误差
R1=0.55;%第1量测误差
R2=0.35; %第2量测误差
w=sqrt(Q)*randn(1,N);
v1=sqrt(R1)*randn(1,N);
v2=sqrt(R2)*randn(1,N);
Beta1=0.5;
Beta2=0.5;
%------------------分配空间---------------------
x=zeros(1,N);
z1=zeros(1,N);
z2=zeros(1,N);
xkf1=zeros(1,N);
xkf2=zeros(1,N);
xkf=zeros(1,N);
xkf_1=zeros(1,N);
xkf_2=zeros(1,N);
p1=zeros(1,N);
p2=zeros(1,N);
p=zeros(1,N);
p_1=zeros(1,N);
p_2=zeros(1,N);
p_pre1=zeros(1,N);
p_pre2=zeros(1,N);
x_pre1=zeros(1,N);
x_pre2=zeros(1,N);
p_pre_1=zeros(1,N);
p_pre_2=zeros(1,N);
x_pre_1=zeros(1,N);
x_pre_2=zeros(1,N);
%------------------初始化---------------------
x(1)=10;
p_1(1)=1;
p_2(1)=1;
p(1)=1;
z1(1)=10;
z2(1)=10;
xkf1(1)=z1(1);
xkf2(1)=z2(1);
xkf=z1(1);
xkf_1(1)=z1(1);
xkf_2(1)=z2(1);
I=eye(1);
%-----------------联邦过程----------------------
for k=1:N-1
x(k+1)=A*x(k)+w(k+1);
z1(k+1)=H1*x(k+1)+v1(k+1);
z2(k+1)=H2*x(k+1)+v2(k+1);
%-----------------作为对比的单独滤波----------------------
p_pre_1(k+1)=A*p_1(k)*A'+Q;
p_pre_2(k+1)=A*p_2(k)*A'+Q;
Kk_1= p_pre_1(k+1)*H1'/(H1* p_pre_1(k+1)*H1'+R1);
Kk_2= p_pre_2(k+1)*H2'/(H2* p_pre_2(k+1)*H2'+R2);
x_pre_1(k+1)=A*xkf_1(k);
x_pre_2(k+1)=A*xkf_2(k);
xkf_1(k+1)=x_pre_1(k+1)+Kk_1*(z1(k+1)-H1*x_pre_1(k+1));
xkf_2(k+1)=x_pre_2(k+1)+Kk_2*(z2(k+1)-H2*x_pre_2(k+1));
p_1(k+1)=(I-Kk_1*H1)*p_pre_1(k+1);
p_2(k+1)=(I-Kk_2*H2)*p_pre_2(k+1);
%-----------------联邦滤波----------------------
%协方差重置
p1(k)=p(k)/Beta1;
p2(k)=p(k)/Beta2;
%信息分配
Q1=Q/Beta1;
Q2=Q/Beta2;
%时间更新（因为参量全部被融合中心重置，因此可在融合中心完成）
p_pre1(k+1)=A*p1(k)*A'+Q1;%same
p_pre2(k+1)=A*p2(k)*A'+Q2;
x_pre1(k+1)=A*xkf1(k);%same
x_pre2(k+1)=A*xkf2(k);
%各部独立进行量测更新
Kk1= p_pre1(k+1)*H1'/(H1* p_pre1(k+1)*H1'+R1); %different
Kk2= p_pre2(k+1)*H2'/(H2* p_pre2(k+1)*H2'+R2);
% Kk1= p_pre1(k+1)*H1'/(H1* p_pre1(k+1)*H1'); %different
% Kk2= p_pre2(k+1)*H2'/(H2* p_pre2(k+1)*H2');
p1(k+1)=(I-Kk1*H1)*p_pre1(k+1); %different
p2(k+1)=(I-Kk2*H2)*p_pre2(k+1);
% p1(k+1)=inv(inv(p_pre1(k+1))+H1*inv(R1)*H1'); %different
% p2(k+1)=inv(inv(p_pre2(k+1))+H2*inv(R2)*H2');
xkf1(k+1)=x_pre1(k+1)+Kk1*(z1(k+1)-H1*x_pre1(k+1)); %different
xkf2(k+1)=x_pre2(k+1)+Kk2*(z2(k+1)-H2*x_pre2(k+1));
%按不相关融合
p(k+1)=inv(inv(p1(k+1))+inv(p2(k+1)));
xkf(k+1)=p(k+1)*(inv(p1(k+1))*xkf1(k+1)+inv(p2(k+1))*xkf2(k+1));
%局部估计重置
xkf1(k+1)=xkf(k+1);
xkf2(k+1)=xkf(k+1);
end
%--------------------------误差计算--------------------------------
%初始化
kalman_err1=zeros(1,N);
kalman_err2=zeros(1,N);
kalman_err=zeros(1,N);
for k=1:N
kalman_err1(k)=abs(xkf_1(k)-x(k));%第1个局部滤波器对第1个状态量估计偏差
kalman_err2(k)=abs(xkf_2(k)-x(k));%第2个局部滤波器对第2个状态量估计偏差
kalman_err(k)=abs(xkf(k)-x(k));%第2个局部滤波器对第2个状态量估计偏差
end
%--------------------------作图--------------------------------
figure;
hold on,box on;
plot(x,'-k*');
plot(xkf_1,'-g.');
plot(xkf_2,'-b.');
plot(xkf,'-r.');
legend('真实','局部1','局部2','全局');
xlabel('采样时间');ylabel('误差均方值');
title('跟踪情况');
figure;
hold on,box on;
plot(kalman_err1,'-g.');
plot(kalman_err2,'-b.');
plot(kalman_err,'-r.');
legend('估计1误差','估计2误差','融合后误差');
xlabel('采样时间');ylabel('误差');
title('滤波器误差');
figure;
hold on,box on;
plot(p_1,'-g.');
plot(p_2,'-b.');
plot(p,'-r.');
legend('估计1误差','估计2误差','融合后误差');
xlabel('采样时间');ylabel('误差');
title('误差协方差');
结果及分析
跟踪效果
跟踪误差
看得出来，融合后的全局滤波误差要比单独的局部误差稍低一点点。
