基于协方差交叉(CI)的多传感器融合算法matlab仿真,对比单传感器和SCC融合

clc; clear; close all;

%% 参数设置
dt = 1;               % 采样时间
T = 50;               % 仿真步数
x0 = [0; 0];          % 初始状态 [位置; 速度]
F = [1 dt; 0 1];      % 状态转移矩阵
Q = 0.01*[dt^3/3 dt^2/2; dt^2/2 dt];  % 系统噪声协方差

% 多传感器设置
R1 = 0.5^2;   % 传感器1测量噪声方差
R2 = 1^2;     % 传感器2测量噪声方差
H = [1 0];    % 测量矩阵

%% 初始化
x_true = zeros(2,T);
x_true(:,1) = x0;
z1 = zeros(1,T); z2 = zeros(1,T);

% 单传感器卡尔曼滤波初始化
x1_hat = zeros(2,T); P1 = eye(2);
x2_hat = zeros(2,T); P2 = eye(2);

% SCC初始化（假设传感器协方差已知）
x_scc = zeros(2,T); Pscc = eye(2);

% CI融合初始化
x_ci = zeros(2,T); Pci = eye(2);

%% 生成真实轨迹和测量
rng(0);
for k = 2:T
    x_true(:,k) = F*x_true(:,k-1) + mvnrnd([0;0], Q)';
    z1(k) = H*x_true(:,k) + sqrt(R1)*randn;
    z2(k) = H*x_true(:,k) + sqrt(R2)*randn;
end

%% 仿真循环
for k = 2:T
    %% 单传感器卡尔曼滤波器
    % 传感器1
    % 预测
    x1_pred = F*x1_hat(:,k-1);
    P1_pred = F*P1*F' + Q;
    % 更新
    K1 = P1_pred*H'/(H*P1_pred*H' + R1);
    x1_hat(:,k) = x1_pred + K1*(z1(k) - H*x1_pred);
    P1 = (eye(2) - K1*H)*P1_pred;
    
    % 传感器2
    x2_pred = F*x2_hat(:,k-1);
    P2_pred = F*P2*F' + Q;
    K2 = P2_pred*H'/(H*P2_pred*H' + R2);
    x2_hat(:,k) = x2_pred + K2*(z2(k) - H*x2_pred);
    P2 = (eye(2) - K2*H)*P2_pred;
    
    %% SCC融合（简单加权融合）
    % 假设协方差已知，使用最小方差加权
    P_scc_inv = inv(P1) + inv(P2);
    Pscc = inv(P_scc_inv);
    x_scc(:,k) = Pscc * (inv(P1)*x1_hat(:,k) + inv(P2)*x2_hat(:,k));
    
    %% CI融合
    % CI算法
    % 计算最优权重w，简单用0.5示例，也可优化
    w = 0.5;
    Pci = inv(w*inv(P1) + (1-w)*inv(P2));
    x_ci(:,k) = Pci * (w*inv(P1)*x1_hat(:,k) + (1-w)*inv(P2)*x2_hat(:,k));
end

%% 绘图对比
figure;
plot(1:T, x_true(1,:), 'k', 'LineWidth',2); hold on;
plot(1:T, x1_hat(1,:), 'r--','LineWidth',1.5);
plot(1:T, x2_hat(1,:), 'b--','LineWidth',1.5);
plot(1:T, x_scc(1,:), 'g-.','LineWidth',1.5);
plot(1:T, x_ci(1,:), 'm','LineWidth',1.5);
legend('真实轨迹','传感器1','传感器2','SCC融合','CI融合');
xlabel('时间步'); ylabel('位置');
title('多传感器融合对比');
grid on;