明白，你想做一个 基于协方差交叉（Covariance Intersection, CI）的多传感器融合算法仿真，并与 单传感器结果和 SCC（Sensor Cross Covariance, 假设这里是标准传感器协方差融合）融合 进行对比。下面我给你一个 完整 MATLAB 仿真示例框架，包括生成多传感器数据、CI融合、SCC融合和对比绘图。

MATLAB 仿真示例

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;

说明

  1. 系统模型
    • 二维状态 [位置; 速度]，使用线性动态模型。
    • 测量矩阵 H 只测位置。
  2. 单传感器滤波
    • 对每个传感器单独运行卡尔曼滤波。
  3. SCC融合
    • 这里假设已知传感器协方差，使用最小方差加权。
  4. CI融合
    • 协方差交叉融合公式：
    PCI=(wP1−1+(1−w)P2−1)−1,xCI=PCI(wP1−1×1+(1−w)P2−1×2)P_{CI} = (w P_1^{-1} + (1-w) P_2^{-1})^{-1}, \quad x_{CI} = P_{CI} (w P_1^{-1} x_1 + (1-w) P_2^{-1} x_2)
    • 权重 w 可通过优化选择。
  5. 结果可视化
    • 绘制真实轨迹、单传感器估计、SCC融合、CI融合进行对比。

明白，我帮你把 CI 融合权重自适应优化加到 MATLAB 仿真中，并生成 均方误差（MSE）对比曲线，直观显示 CI 融合相比单传感器和 SCC 的优势。

自适应 CI 权重 MATLAB 仿真示例

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; R2 = 1^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);

x_scc = zeros(2,T); Pscc = eye(2);
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
    %% 单传感器卡尔曼滤波
    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;

    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融合（自适应权重）
    % 权重 w 优化（最小迹）
    w_candidates = 0:0.01:1;
    min_trace = Inf; best_w = 0.5;
    for w = w_candidates
        P_temp = inv(w*inv(P1) + (1-w)*inv(P2));
        if trace(P_temp) < min_trace
            min_trace = trace(P_temp);
            best_w = w;
        end
    end
    % CI融合
    w = best_w;
    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

%% 均方误差计算
mse1 = mean((x1_hat(1,:)-x_true(1,:)).^2);
mse2 = mean((x2_hat(1,:)-x_true(1,:)).^2);
mse_scc = mean((x_scc(1,:)-x_true(1,:)).^2);
mse_ci = mean((x_ci(1,:)-x_true(1,:)).^2);

fprintf('MSE: Sensor1=%.4f, Sensor2=%.4f, SCC=%.4f, CI=%.4f\n', mse1, mse2, mse_scc, mse_ci);

%% 绘图
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('多传感器融合对比（自适应CI）');
grid on;

%% MSE对比柱状图
figure;
bar([mse1 mse2 mse_scc mse_ci]);
set(gca,'xticklabel',{'Sensor1','Sensor2','SCC','CI'});
ylabel('均方误差(MSE)');
title('各方法MSE对比');
grid on;

说明

  1. 自适应权重 w
    • CI 融合的权重通过 最小化协方差迹 (trace(P)) 来选取最佳 w。
    • 避免了手动选择 w，提升融合精度。
  2. MSE对比
    • 仿真计算各方法位置误差均方值，柱状图直观展示融合优势。
    • CI 融合通常比单传感器和 SCC 更优。
  3. 可扩展
    • 支持更多传感器，只需扩展 CI 融合权重搜索和 SCC 加权。
    • 可改为非线性模型，配合 EKF 或 UKF 处理。