% EKF 参数初始化
dt = 0.1;  % 时间间隔
F = [1, dt, 0; 0, 1, dt; 0, 0, 1];  % 状态转移矩阵
H_INS = [1, 0, 0];  % INS 观测矩阵
H_ODOM = [1, 0, 0]; % ODOM 观测矩阵
Q = 0.01 * eye(3);  % 过程噪声协方差
R_INS = 0.05;       % INS 观测噪声协方差
R_ODOM = 0.1;       % ODOM 观测噪声协方差

x = [0; 0; 0];      % 初始状态 [位置; 速度; 加速度]
P = eye(3);         % 初始估计误差协方差

% 模拟真实系统和传感器观测
num_steps = 100;                      % 时间步数
true_state = zeros(3, num_steps);      % 真实状态 [位置; 速度; 加速度]
observations_INS = zeros(1, num_steps); % INS 观测值
observations_ODOM = zeros(1, num_steps); % ODOM 观测值
IMU_accel = zeros(1, num_steps);        % IMU 加速度

for t = 2:num_steps
    % 真实状态更新
    true_state(:, t) = F * true_state(:, t-1) + [0; 0; 0.1] * randn;  % 加入过程噪声

    % 传感器观测更新
    observations_INS(t) = H_INS * true_state(:, t) + sqrt(R_INS) * randn;  % INS 噪声
    observations_ODOM(t) = H_ODOM * true_state(:, t) + sqrt(R_ODOM) * randn; % ODOM 噪声
    IMU_accel(t) = true_state(3, t) + sqrt(0.05) * randn;  % IMU 加速度噪声
end

% EKF 过程
estimates = zeros(3, num_steps);  % 状态估计

for t = 2:num_steps
    % ---------- 预测步骤 ----------
    % 状态预测
    x_pred = F * x;
    P_pred = F * P * F' + Q;

    % ---------- 更新步骤 (INS) ----------
    % INS 传感器的更新
    K_INS = P_pred * H_INS' / (H_INS * P_pred * H_INS' + R_INS);  % 卡尔曼增益（INS）
    x = x_pred + K_INS * (observations_INS(t) - H_INS * x_pred);  % 状态更新
    P = (eye(3) - K_INS * H_INS) * P_pred;                         % 误差协方差更新

    % ---------- 更新步骤 (ODOM) ----------
    % ODOM 传感器的更新
    K_ODOM = P_pred * H_ODOM' / (H_ODOM * P_pred * H_ODOM' + R_ODOM);  % 卡尔曼增益（ODOM）
    x = x + K_ODOM * (observations_ODOM(t) - H_ODOM * x);  % 状态更新
    P = (eye(3) - K_ODOM * H_ODOM) * P;                     % 误差协方差更新

    % ---------- 使用 IMU 的加速度信息 ----------
    % IMU 数据的加速度直接用于状态更新
    x(3) = IMU_accel(t);  % 直接更新加速度状态

    estimates(:, t) = x;  % 保存状态估计
end

% 可视化
figure;
subplot(3,1,1);
plot(1:num_steps, true_state(1, :), '-b', 'DisplayName', '真实位置');
hold on;
plot(1:num_steps, estimates(1, :), '-r', 'DisplayName', '估计位置');
legend;
xlabel('时间步');
ylabel('位置');

subplot(3,1,2);
plot(1:num_steps, true_state(2, :), '-b', 'DisplayName', '真实速度');
hold on;
plot(1:num_steps, estimates(2, :), '-r', 'DisplayName', '估计速度');
legend;
xlabel('时间步');
ylabel('速度');

subplot(3,1,3);
plot(1:num_steps, true_state(3, :), '-b', 'DisplayName', '真实加速度');
hold on;
plot(1:num_steps, estimates(3, :), '-r', 'DisplayName', '估计加速度');
legend;
xlabel('时间步');
ylabel('加速度');