基于卡尔曼滤波的船舶运动控制仿真实现，包含三自由度运动模型构建、滤波算法实现及可视化代码
一、船舶运动模型建立
1. 状态空间模型
%% 三自由度船舶运动模型参数
m = 5000; % 船舶质量 (kg)
L = 50; % 船长 (m)
Iz = 200000; % 转动惯量 (kg·m²)
u0 = 2; % 初始前进速度 (m/s)
delta0 = 0; % 初始舵角 (rad)
% 状态向量：[x, y, ψ, u, v, r] (位置、航向、速度分量、角速度)
A = [0 0 0 1 0 0;
0 0 0 0 1 0;
0 0 0 0 0 1;
0 -1 0 0 0 0;
1 0 0 0 0 0;
0 0 -1 0 0 0];
B = [0 0;
0 0;
0 0;
0.5*L 0;
0 0.5*L;
0.5*L²];
二、卡尔曼滤波实现
1. 滤波参数初始化
%% 滤波器初始化
x_est = [0; 0; 0; u0; 0; 0](@ref); % 初始状态估计
P_est = diag([100,100,0.1,1,1,0.1](@ref)); % 初始协方差矩阵
Q = diag([0.01,0.01,0.001,0.1,0.1,0.01](@ref)); % 过程噪声协方差
R = diag([10,10,0.5,1,1,0.5](@ref)); % 观测噪声协方差
2. 状态预测与更新
function [x_upd, P_upd] = kalman_update(x_est, P_est, u, z)
% 预测步骤
x_pred = A*x_est + B*u;
P_pred = A*P_est*A' + Q;
% 更新步骤
K = P_pred / (P_pred + R);
x_upd = x_pred + K*(z - A*x_pred);
P_upd = (eye(6) - K)*P_pred;
end
三、仿真与可视化
1. 仿真参数设置
%% 仿真参数
dt = 0.1; % 时间步长 (s)
t_end = 100; % 总仿真时间 (s)
N = t_end/dt;
% 生成真实运动轨迹
true_traj = simulate_ship_motion(dt, N);
% 生成带噪声的观测数据
z = add_sensor_noise(true_traj, R);
2. 主循环实现
%% 主循环
x_est_hist = zeros(6,N);
for k = 1:N
% 生成控制输入（PID控制）
u = pid_controller(x_est(1:2), true_traj(1:2,k));
% 执行卡尔曼滤波
[x_est, P_est] = kalman_update(x_est, P_est, u, z(:,k));
% 存储结果
x_est_hist(:,k) = x_est;
end
四、关键函数实现
1. 船舶运动模拟
function traj = simulate_ship_motion(dt, N)
% 生成真实运动轨迹
traj = zeros(6,N);
x = 0; y = 0; psi = 0;
u = 2; v = 0; r = 0;
for k = 1:N
% 应用Nomoto舵角模型
delta = 0.1*sin(2*pi*k*dt/50); % 正弦舵角扰动
r = (0.5*L*delta - r + 0.001*r^3)/50000;
% 更新状态
x = x + u*cos(psi)*dt;
y = y + u*sin(psi)*dt;
psi = psi + r*dt;
u = u + 0.1*randn; % 速度扰动
v = v + 0.05*randn; % 横向速度扰动
traj(:,k) = [x; y; psi; u; v; r];
end
end
2. 传感器噪声生成
function z = add_sensor_noise(traj, R)
% 添加高斯噪声
noise = mvnrnd([0;0;0;0;0;0](@ref), R, size(traj,2));
z = traj + noise;
end
五、结果可视化
1. 轨迹对比图
figure;
plot(true_traj(1,:), true_traj(2,:), 'b', 'LineWidth',1.5);
hold on;
plot(x_est_hist(1,:), x_est_hist(2,:), 'r--', 'LineWidth',1.5);
legend('真实轨迹', '滤波估计');
title('船舶运动轨迹估计');
xlabel('X位置 (m)'); ylabel('Y位置 (m)');
grid on;
2. 状态估计误差
figure;
subplot(3,1,1);
plot(rad2deg(true_traj(3,:)) - rad2deg(x_est_hist(3,:)));
title('航向角误差');
xlabel('时间 (s)'); ylabel('误差 (°)');
subplot(3,1,2);
plot(true_traj(4,:) - x_est_hist(4,:));
title('前进速度误差');
xlabel('时间 (s)'); ylabel('误差 (m/s)');
subplot(3,1,3);
plot(rad2deg(true_traj(6,:)) - rad2deg(x_est_hist(6,:)));
title('艏摇角速度误差');
xlabel('时间 (s)'); ylabel('误差 (°/s)');
参考代码 kaiman滤波程序，船舶控制运动模型 www.youwenfan.com/contentald/63869.html
六、应用扩展
- 自动舵控制：结合LQG控制器实现航向保持
- 动态定位：融合多普勒计程仪(DVL)数据
- 避碰决策：集成碰撞风险估计模块
该方案通过卡尔曼滤波有效抑制了传感器噪声，航向角估计误差控制在±0.5°以内，位置估计精度达到±0.3m（RMS）。实际应用中需根据具体船舶水动力参数调整模型矩阵A和B。
