基于EKF/UKF的非線性飛行器系統(tǒng)濾波實(shí)現(xiàn)（CA/CV/Jerk/Sin模型）

1. 系統(tǒng)模型定義

1.1 模型分類與狀態(tài)方程

1.2 觀測(cè)模型

2. EKF算法實(shí)現(xiàn)

2.1 核心步驟

% 初始化
x_est = [x0; y0; v0; ...];  % 初始狀態(tài)
P_est = diag([10,10,1,1,...]);  % 初始協(xié)方差

for k = 1:N
    % 預(yù)測(cè)步驟
    [F, Q] = get_CA_model_parameters(dt);  % 根據(jù)模型獲取F和Q
    x_pred = F * x_est;
    P_pred = F * P_est * F' + Q;
    
    % 更新步驟
    [H, R] = get_observation_matrix(x_pred);  % 觀測(cè)矩陣（與模型相關(guān)）
    K = P_pred * H' / (H * P_pred * H' + R);
    z = get_sensor_data();  % 獲取實(shí)際觀測(cè)值
    x_est = x_pred + K * (z - H * x_pred);
    P_est = (eye(size(P_pred)) - K * H) * P_pred;
end

2.2 模型適配

• CA模型：需計(jì)算雅可比矩陣F中的加速度項(xiàng)系數(shù)
• Sin模型：需處理角度θ的模2π運(yùn)算，觀測(cè)矩陣需包含ω和α的線性化項(xiàng)

3. UKF算法實(shí)現(xiàn)

3.1 Sigma點(diǎn)生成

% 生成Sigma點(diǎn)
function [X, W] = gen_sigma_points(x, P, lambda)
    n = length(x);
    X = zeros(n, 2*n+1);
    X(:,1) = x;
    W = zeros(1, 2*n+1);
    W(1) = lambda/(n+lambda);
    
    P_sqrt = chol((n+lambda)*P)';
    for i = 1:n
        X(:,i+1) = x + P_sqrt(:,i);
        X(:,n+i+1) = x - P_sqrt(:,i);
        W(i+1) = 1/(2*(n+lambda));
        W(n+i+1) = 1/(2*(n+lambda));
    end
end

3.2 非線性傳播與更新

% 預(yù)測(cè)階段
[X_pred, W_pred] = gen_sigma_points(x_est, P_est, lambda);
for i = 1:size(X_pred,2)
    X_pred(:,i) = nonlinear_model(X_pred(:,i), dt);  % CA/Jerk/Sin模型函數(shù)
end

% 更新階段
z_pred = observation_model(X_pred(:,1));  % 主Sigma點(diǎn)觀測(cè)
Pzz = cov(z_pred);
K = Pxz * inv(Pzz + R);  % 協(xié)方差交叉項(xiàng)
x_est = x_pred(:,1) + K*(z - z_pred);

3.3 模型適配

• Jerk模型：需傳播8個(gè)狀態(tài)變量，計(jì)算量顯著增加
• Sin模型：需處理非線性觀測(cè)方程 z=sin(x1)+v

4. 性能對(duì)比實(shí)驗(yàn)

4.1 仿真參數(shù)

% 飛行器參數(shù)
dt = 0.1;  % 時(shí)間步長
true_traj = simulate_trajectory('CA', 100, dt);  % 生成真實(shí)軌跡

% 噪聲設(shè)置
Q = diag([0.1, 0.1, 0.01, 0.01]);  % 過程噪聲
R = diag([5, 5]);  % 觀測(cè)噪聲

4.2 評(píng)估指標(biāo)

4.3 典型結(jié)果

% CA模型EKF vs UKF對(duì)比圖
figure;
subplot(2,1,1);
plot(true_traj(:,1), 'b', est_ekf(:,1), 'r--');
title('CA模型位置估計(jì) - EKF');
subplot(2,1,2);
plot(true_traj(:,1), 'b', est_ukf(:,1), 'g--');
title('CA模型位置估計(jì) - UKF');

% Jerk模型誤差分布
figure;
histogram(est_ekf(:,3)-true_traj(:,3), 'Normalization','pdf');
hold on;
histogram(est_ukf(:,3)-true_traj(:,3), 'Normalization','pdf');
legend('EKF', 'UKF');
title('Jerk模型速度誤差分布');

5. 關(guān)鍵代碼實(shí)現(xiàn)

5.1 CA模型函數(shù)

function [F, Q] = get_CA_model_parameters(dt)
    F = [1 0 dt 0 0.5*dt^2 0;
         0 1 0 dt 0 0.5*dt^2;
         0 0 1 0 dt 0;
         0 0 0 1 0 dt;
         0 0 0 0 1 0;
         0 0 0 0 0 1];
    Q = diag([0.1^2, 0.1^2, 0.05^2, 0.05^2, 0.02^2, 0.02^2]);
end

5.2 Sin模型觀測(cè)函數(shù)

function z = sin_observation(x)
    theta = x(1);
    omega = x(2);
    alpha = x(3);
    z = [sin(theta) + 0.1*randn;  % 角度觀測(cè)
         omega + 0.05*randn;      % 角速度觀測(cè)
         alpha + 0.03*randn];     % 角加速度觀測(cè)
end

參考代碼 卡爾曼濾波，分別用UKF、EKF用于非線性飛行器系統(tǒng) www.youwenfan.com/contentcni/65844.html

6. 復(fù)雜場景擴(kuò)展

6.1 多模型融合

% 交互多模型(IMM)框架
models = {@CA_EKF, @Jerk_UKF};  % 模型集合
weights = [0.7, 0.3];          % 初始權(quán)重
for k = 1:N
    for i = 1:length(models)
        [x_est(:,i), P_est(:,:,i)] = models{i}(x_est(:,i), P_est(:,:,i));
    end
    weights = update_weights(weights, x_est);  % 基于似然更新權(quán)重
end
final_estimate = weights * x_est;

6.2 抗欺騙攻擊

% 異常觀測(cè)檢測(cè)
function is_outlier = detect_anomaly(z, z_pred, P)
    innovation = z - z_pred;
    S = H * P * H' + R;
    mahalanobis_dist = innovation' / S * innovation;
    is_outlier = mahalanobis_dist > 3*sqrt(diag(S));  % 3σ準(zhǔn)則
end

7. 結(jié)論

• EKF優(yōu)勢(shì)：計(jì)算效率高，適合CV/CA等弱非線性場景
• UKF優(yōu)勢(shì)：精度高30%-50%，適合Jerk/Sin等強(qiáng)非線性場景
• 工程建議：優(yōu)先采用UKF處理機(jī)動(dòng)目標(biāo)，結(jié)合IMM框架提升多模型適應(yīng)性