卡尔曼滤波在船舶运动控制中的应用
使用MATLAB实现卡尔曼滤波用于船舶运动控制的代码。
船舶运动模型
首先，我们定义一个三自由度（平面运动）的船舶运动模型，包括位置(x,y)和航向角(ψ)。
% 船舶运动模型与卡尔曼滤波
clear; clc; close all;
% 船舶参数
m = 1000; % 质量 (kg)
Iz = 500; % 绕z轴的转动惯量 (kg*m^2)
Xu = -50; % 纵向阻尼系数
Yv = -100; % 横向阻尼系数
Nr = -100; % 转首阻尼系数
% 采样时间
dt = 0.1; % 时间步长
T = 0:dt:100; % 时间向量
n = length(T);
% 初始状态: [x, y, psi, u, v, r]
state = zeros(6, n);
state(:, 1) = [0; 0; 0; 0; 0; 0]; % 初始状态
% 控制输入: [X, Y, N] 力和力矩
control = zeros(3, n);
% 模拟控制输入 - 简单的正弦波控制
control(1, :) = 100 * sin(0.1*T); % 纵向力
control(2, :) = 50 * cos(0.05*T); % 横向力
control(3, :) = 200 * sin(0.02*T); % 转首力矩
% 过程噪声协方差
Q = diag([0.1, 0.1, 0.01, 0.1, 0.1, 0.01]); % 状态噪声
% 船舶运动模拟
for i = 1:n-1
% 当前状态
x = state(1, i);
y = state(2, i);
psi = state(3, i);
u = state(4, i);
v = state(5, i);
r = state(6, i);
% 控制输入
X = control(1, i);
Y = control(2, i);
N = control(3, i);
% 船舶运动方程 (三自由度)
u_dot = (X - Xu*u)/m + v*r;
v_dot = (Y - Yv*v)/m - u*r;
r_dot = (N - Nr*r)/Iz;
% 更新速度
u_new = u + u_dot * dt;
v_new = v + v_dot * dt;
r_new = r + r_dot * dt;
% 更新位置和航向
x_new = x + (u*cos(psi) - v*sin(psi)) * dt;
y_new = y + (u*sin(psi) + v*cos(psi)) * dt;
psi_new = psi + r * dt;
% 归一化航向角到[-pi, pi]
psi_new = atan2(sin(psi_new), cos(psi_new));
% 添加过程噪声
noise = sqrt(Q) * randn(6, 1);
% 更新状态
state(:, i+1) = [x_new; y_new; psi_new; u_new; v_new; r_new] + noise;
end
% 绘制真实轨迹
figure;
plot(state(1,:), state(2,:));
title('船舶真实轨迹');
xlabel('X位置 (m)');
ylabel('Y位置 (m)');
grid on;
axis equal;
卡尔曼滤波实现
现在我们在船舶运动模型的基础上添加卡尔曼滤波进行状态估计。
% 测量设置 - 假设我们只能测量位置和航向
H = [1 0 0 0 0 0; % x位置
0 1 0 0 0 0; % y位置
0 0 1 0 0 0]; % 航向
measurement_dim = size(H, 1);
% 测量噪声协方差
R = diag([1, 1, 0.1]); % 测量噪声
% 生成测量数据
measurements = H * state + sqrt(R) * randn(measurement_dim, n);
% 初始化卡尔曼滤波
x_est = zeros(6, n); % 状态估计
P_est = zeros(6, 6, n); % 估计协方差
% 初始估计
x_est(:, 1) = [0; 0; 0; 0; 0; 0];
P_est(:, :, 1) = eye(6);
% 卡尔曼滤波循环
for i = 1:n-1
% 当前估计
x = x_est(:, i);
P = P_est(:, :, i);
% 状态转移矩阵 (线性化模型)
psi = x(3);
u = x(4);
v = x(5);
r = x(6);
% 雅可比矩阵 (状态转移矩阵)
A = eye(6);
A(1, 3) = (-u*sin(psi) - v*cos(psi)) * dt;
A(1, 4) = cos(psi) * dt;
A(1, 5) = -sin(psi) * dt;
A(2, 3) = (u*cos(psi) - v*sin(psi)) * dt;
A(2, 4) = sin(psi) * dt;
A(2, 5) = cos(psi) * dt;
A(3, 6) = dt;
A(4, 5) = r * dt;
A(4, 6) = v * dt;
A(5, 4) = -r * dt;
A(5, 6) = -u * dt;
% 控制输入矩阵
B = zeros(6, 3);
B(4, 1) = dt/m;
B(5, 2) = dt/m;
B(6, 3) = dt/Iz;
% 预测步骤
x_pred = x;
x_pred(1) = x(1) + (u*cos(psi) - v*sin(psi)) * dt;
x_pred(2) = x(2) + (u*sin(psi) + v*cos(psi)) * dt;
x_pred(3) = x(3) + r * dt;
x_pred(4) = x(4) + (control(1, i) - Xu*u)/m * dt + v*r*dt;
x_pred(5) = x(5) + (control(2, i) - Yv*v)/m * dt - u*r*dt;
x_pred(6) = x(6) + (control(3, i) - Nr*r)/Iz * dt;
% 归一化航向角
x_pred(3) = atan2(sin(x_pred(3)), cos(x_pred(3)));
P_pred = A * P * A' + Q;
% 更新步骤
z = measurements(:, i+1);
y = z - H * x_pred; % 测量残差
S = H * P_pred * H' + R; % 残差协方差
K = P_pred * H' / S; % 卡尔曼增益
x_est(:, i+1) = x_pred + K * y;
P_est(:, :, i+1) = (eye(6) - K * H) * P_pred;
end
% 绘制结果比较
figure;
% 位置轨迹比较
subplot(2,2,1);
plot(state(1,:), state(2,:), 'b', 'LineWidth', 1.5);
hold on;
plot(measurements(1,:), measurements(2,:), 'ro', 'MarkerSize', 3);
plot(x_est(1,:), x_est(2,:), 'g--', 'LineWidth', 1.5);
title('船舶轨迹');
xlabel('X位置 (m)');
ylabel('Y位置 (m)');
legend('真实轨迹', '测量值', '卡尔曼估计');
grid on;
axis equal;
% 航向角比较
subplot(2,2,2);
plot(T, state(3,:), 'b', 'LineWidth', 1.5);
hold on;
plot(T, measurements(3,:), 'ro', 'MarkerSize', 3);
plot(T, x_est(3,:), 'g--', 'LineWidth', 1.5);
title('航向角');
xlabel('时间 (s)');
ylabel('航向角 (rad)');
legend('真实值', '测量值', '卡尔曼估计');
grid on;
% 速度比较 - 纵向速度
subplot(2,2,3);
plot(T, state(4,:), 'b', 'LineWidth', 1.5);
hold on;
plot(T, x_est(4,:), 'g--', 'LineWidth', 1.5);
title('纵向速度');
xlabel('时间 (s)');
ylabel('速度 (m/s)');
legend('真实值', '卡尔曼估计');
grid on;
% 速度比较 - 横向速度
subplot(2,2,4);
plot(T, state(5,:), 'b', 'LineWidth', 1.5);
hold on;
plot(T, x_est(5,:), 'g--', 'LineWidth', 1.5);
title('横向速度');
xlabel('时间 (s)');
ylabel('速度 (m/s)');
legend('真实值', '卡尔曼估计');
grid on;
% 计算估计误差
position_error = sqrt((state(1,:) - x_est(1,:)).^2 + (state(2,:) - x_est(2,:)).^2);
heading_error = abs(state(3,:) - x_est(3,:));
% 显示平均误差
fprintf('平均位置误差: %.4f m\n', mean(position_error));
fprintf('平均航向误差: %.4f rad\n', mean(heading_error));
扩展卡尔曼滤波（EKF）实现
对于非线性系统，我们可以使用扩展卡尔曼滤波（EKF）：
% 扩展卡尔曼滤波（EKF）实现
x_est_ekf = zeros(6, n); % 状态估计
P_est_ekf = zeros(6, 6, n); % 估计协方差
% 初始估计
x_est_ekf(:, 1) = [0; 0; 0; 0; 0; 0];
P_est_ekf(:, :, 1) = eye(6);
% EKF循环
for i = 1:n-1
% 当前估计
x = x_est_ekf(:, i);
P = P_est_ekf(:, :, i);
% 预测步骤 (使用非线性模型)
psi = x(3);
u = x(4);
v = x(5);
r = x(6);
% 非线性状态预测
x_pred = x;
x_pred(1) = x(1) + (u*cos(psi) - v*sin(psi)) * dt;
x_pred(2) = x(2) + (u*sin(psi) + v*cos(psi)) * dt;
x_pred(3) = x(3) + r * dt;
x_pred(4) = x(4) + (control(1, i) - Xu*u)/m * dt + v*r*dt;
x_pred(5) = x(5) + (control(2, i) - Yv*v)/m * dt - u*r*dt;
x_pred(6) = x(6) + (control(3, i) - Nr*r)/Iz * dt;
% 归一化航向角
x_pred(3) = atan2(sin(x_pred(3)), cos(x_pred(3)));
% 计算雅可比矩阵 (状态转移矩阵)
A = eye(6);
A(1, 3) = (-u*sin(psi) - v*cos(psi)) * dt;
A(1, 4) = cos(psi) * dt;
A(1, 5) = -sin(psi) * dt;
A(2, 3) = (u*cos(psi) - v*sin(psi)) * dt;
A(2, 4) = sin(psi) * dt;
A(2, 5) = cos(psi) * dt;
A(3, 6) = dt;
A(4, 5) = r * dt;
A(4, 6) = v * dt;
A(5, 4) = -r * dt;
A(5, 6) = -u * dt;
% 预测协方差
P_pred = A * P * A' + Q;
% 更新步骤
z = measurements(:, i+1);
y = z - H * x_pred; % 测量残差
S = H * P_pred * H' + R; % 残差协方差
K = P_pred * H' / S; % 卡尔曼增益
x_est_ekf(:, i+1) = x_pred + K * y;
P_est_ekf(:, :, i+1) = (eye(6) - K * H) * P_pred;
end
% 比较标准KF和EKF的性能
figure;
subplot(2,1,1);
plot(T, position_error, 'b', 'LineWidth', 1.5);
hold on;
position_error_ekf = sqrt((state(1,:) - x_est_ekf(1,:)).^2 + (state(2,:) - x_est_ekf(2,:)).^2);
plot(T, position_error_ekf, 'r--', 'LineWidth', 1.5);
title('位置估计误差');
xlabel('时间 (s)');
ylabel('误差 (m)');
legend('标准KF', 'EKF');
grid on;
subplot(2,1,2);
plot(T, heading_error, 'b', 'LineWidth', 1.5);
hold on;
heading_error_ekf = abs(state(3,:) - x_est_ekf(3,:));
plot(T, heading_error_ekf, 'r--', 'LineWidth', 1.5);
title('航向估计误差');
xlabel('时间 (s)');
ylabel('误差 (rad)');
legend('标准KF', 'EKF');
grid on;
% 显示平均误差
fprintf('EKF平均位置误差: %.4f m\n', mean(position_error_ekf));
fprintf('EKF平均航向误差: %.4f rad\n', mean(heading_error_ekf));
参考代码 kaiman滤波程序，船舶控制运动模型 www.3dddown.com/csa/63869.html
使用
- 上述代码首先定义了一个三自由度的船舶运动模型，包括位置(x,y)和航向角(ψ)以及相应的速度。
- 然后实现了标准卡尔曼滤波和扩展卡尔曼滤波(EKF)进行状态估计。
- 代码比较了真实状态、测量值和估计值，并计算了估计误差。
注意
- 船舶运动模型可以根据实际船舶参数进行调整。
- 过程噪声协方差Q和测量噪声协方差R需要根据实际系统的噪声特性进行调整。
- 对于高度非线性的系统，可能需要使用无迹卡尔曼滤波(UKF)或其他非线性滤波方法。
- 在实际应用中，可能需要考虑更多的状态变量和更复杂的船舶动力学模型。
浙公网安备 33010602011771号