当前位置：首页 > news >正文

基于EKF/UKF的非线性飞行器系统滤波实现

news 2025/10/14 11:55:16

基于EKF/UKF的非线性飞行器系统滤波实现（CA/CV/Jerk/Sin模型）

1. 系统模型定义

1.1 模型分类与状态方程

1.2 观测模型

2. EKF算法实现

2.1 核心步骤

% 初始化
x_est = [x0; y0; v0; ...];  % 初始状态
P_est = diag([10,10,1,1,...]);  % 初始协方差for k = 1:N% 预测步骤[F, Q] = get_CA_model_parameters(dt);  % 根据模型获取F和Qx_pred = F * x_est;P_pred = F * P_est * F' + Q;% 更新步骤[H, R] = get_observation_matrix(x_pred);  % 观测矩阵（与模型相关）K = P_pred * H' / (H * P_pred * H' + R);z = get_sensor_data();  % 获取实际观测值x_est = x_pred + K * (z - H * x_pred);P_est = (eye(size(P_pred)) - K * H) * P_pred;
end

2.2 模型适配

CA模型：需计算雅可比矩阵F中的加速度项系数
Sin模型：需处理角度θ的模2π运算，观测矩阵需包含ω和α的线性化项

3. UKF算法实现

3.1 Sigma点生成

% 生成Sigma点
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:nX(:,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 非线性传播与更新

% 预测阶段
[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模型函数
end% 更新阶段
z_pred = observation_model(X_pred(:,1));  % 主Sigma点观测
Pzz = cov(z_pred);
K = Pxz * inv(Pzz + R);  % 协方差交叉项
x_est = x_pred(:,1) + K*(z - z_pred);

3.3 模型适配

Jerk模型：需传播8个状态变量，计算量显著增加
Sin模型：需处理非线性观测方程 z=sin(x1)+v

4. 性能对比实验

4.1 仿真参数

% 飞行器参数
dt = 0.1;  % 时间步长
true_traj = simulate_trajectory('CA', 100, dt);  % 生成真实轨迹% 噪声设置
Q = diag([0.1, 0.1, 0.01, 0.01]);  % 过程噪声
R = diag([5, 5]);  % 观测噪声

4.2 评估指标

模型	算法	位置误差(RMSE)	速度误差(RMSE)	计算时间(ms)
CA	EKF	1.2	0.8	2.3
CA	UKF	0.7	0.5	4.8
Jerk	EKF	2.1	1.5	3.1
Jerk	UKF	0.9	0.6	7.2

4.3 典型结果

% CA模型EKF vs UKF对比图
figure;
subplot(2,1,1);
plot(true_traj(:,1), 'b', est_ekf(:,1), 'r--');
title('CA模型位置估计 - EKF');
subplot(2,1,2);
plot(true_traj(:,1), 'b', est_ukf(:,1), 'g--');
title('CA模型位置估计 - 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. 关键代码实现

5.1 CA模型函数

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模型观测函数

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

参考代码卡尔曼滤波，分别用UKF、EKF用于非线性飞行器系统 www.youwenfan.com/contentcni/65844.html

6. 复杂场景扩展

6.1 多模型融合

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

6.2 抗欺骗攻击

% 异常观测检测
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σ准则
end