三自由度车辆模型的扩展卡尔曼滤波（EKF）程序

原创于 2025-12-12 16:08:57 发布 · 196 阅读

CC 4.0 BY-SA版权

文章标签：

使用扩展卡尔曼滤波（EKF）来估计三自由度车辆模型的状态。这个模型包括纵向速度、横向速度和横摆角速度。

%% 三自由度车辆模型的扩展卡尔曼滤波程序
clc; clear; close all;

%% 车辆参数设置
m = 1573;       % 车辆质量 (kg)
Iz = 2873;      % 绕Z轴的转动惯量 (kg*m^2)
lf = 1.1;       % 前轴到质心的距离 (m)
lr = 1.58;      % 后轴到质心的距离 (m)
Cf = 80000;     % 前轮侧偏刚度 (N/rad)
Cr = 80000;     % 后轮侧偏刚度 (N/rad)

%% 仿真参数
dt = 0.01;      % 时间步长 (s)
T = 10;         % 总仿真时间 (s)
N = T/dt;       % 总步数

%% 初始状态
% 状态向量: [纵向速度 vx, 横向速度 vy, 横摆角速度 r]
x_true = [20; 0; 0];    % 真实状态 (m/s, m/s, rad/s)
x_est = [19; 0.1; 0.1]; % 估计状态 (初始带有误差)
P = diag([1, 0.1, 0.01]); % 估计误差协方差矩阵

%% 过程噪声和测量噪声
Q = diag([0.1, 0.1, 0.01]); % 过程噪声协方差
R = diag([0.5, 0.5, 0.05]); % 测量噪声协方差

%% 输入控制 (前轮转角和驱动力矩)
% 正弦转向输入
delta = zeros(1, N);
for k = 1:N
    if k < N/2
        delta(k) = 0.1 * sin(2*pi*0.5*k*dt);
    else
        delta(k) = 0.05 * sin(2*pi*0.5*k*dt);
    end
end

% 简单的驱动力矩输入
Fx = 200 * ones(1, N); % 恒定的驱动力 (N)

%% 数据存储
x_true_history = zeros(3, N);
x_est_history = zeros(3, N);
measurements_history = zeros(3, N);
innovation_history = zeros(3, N);

%% 扩展卡尔曼滤波主循环
for k = 1:N
    % 获取当前控制输入
    u = [delta(k); Fx(k)];
    
    % 1. 真实系统动态 (用于生成测量值)
    x_true = vehicle_dynamics(x_true, u, m, Iz, lf, lr, Cf, Cr, dt);
    
    % 2. 生成带有噪声的测量值
    z = measure(x_true) + sqrt(R) * randn(3, 1);
    
    % 3. 预测步骤 (使用非线性模型)
    [x_pred, F] = predict(x_est, u, m, Iz, lf, lr, Cf, Cr, dt);
    P_pred = F * P * F' + Q;
    
    % 4. 更新步骤
    [z_pred, H] = measurement_model(x_pred);
    y = z - z_pred; % 创新向量
    
    S = H * P_pred * H' + R;
    K = P_pred * H' / S;
    
    x_est = x_pred + K * y;
    P = (eye(3) - K * H) * P_pred;
    
    % 存储数据
    x_true_history(:, k) = x_true;
    x_est_history(:, k) = x_est;
    measurements_history(:, k) = z;
    innovation_history(:, k) = y;
end

%% 绘制结果
t = 0:dt:T-dt;

% 状态估计结果
figure('Position', [100, 100, 1200, 800]);
subplot(3,1,1);
plot(t, x_true_history(1,:), 'LineWidth', 2); hold on;
plot(t, x_est_history(1,:), '--', 'LineWidth', 2);
plot(t, measurements_history(1,:), '.', 'MarkerSize', 8);
legend('真实值', '估计值', '测量值');
title('纵向速度 v_x (m/s)');
grid on;

subplot(3,1,2);
plot(t, x_true_history(2,:), 'LineWidth', 2); hold on;
plot(t, x_est_history(2,:), '--', 'LineWidth', 2);
plot(t, measurements_history(2,:), '.', 'MarkerSize', 8);
legend('真实值', '估计值', '测量值');
title('横向速度 v_y (m/s)');
grid on;

subplot(3,1,3);
plot(t, x_true_history(3,:), 'LineWidth', 2); hold on;
plot(t, x_est_history(3,:), '--', 'LineWidth', 2);
plot(t, measurements_history(3,:), '.', 'MarkerSize', 8);
legend('真实值', '估计值', '测量值');
title('横摆角速度 r (rad/s)');
grid on;

% 创新序列 (应表现为零均值白噪声)
figure('Position', [100, 100, 1000, 600]);
subplot(3,1,1);
plot(t, innovation_history(1,:));
title('纵向速度创新序列');
grid on;

subplot(3,1,2);
plot(t, innovation_history(2,:));
title('横向速度创新序列');
grid on;

subplot(3,1,3);
plot(t, innovation_history(3,:));
title('横摆角速度创新序列');
grid on;

% 估计误差
errors = x_est_history - x_true_history;
figure('Position', [100, 100, 1000, 600]);
subplot(3,1,1);
plot(t, errors(1,:));
title('纵向速度估计误差');
grid on;

subplot(3,1,2);
plot(t, errors(2,:));
title('横向速度估计误差');
grid on;

subplot(3,1,3);
plot(t, errors(3,:));
title('横摆角速度估计误差');
grid on;

% 计算并显示均方根误差(RMSE)
rmse_vx = sqrt(mean(errors(1,:).^2));
rmse_vy = sqrt(mean(errors(2,:).^2));
rmse_r = sqrt(mean(errors(3,:).^2));

fprintf('纵向速度估计RMSE: %.4f m/s\n', rmse_vx);
fprintf('横向速度估计RMSE: %.4f m/s\n', rmse_vy);
fprintf('横摆角速度估计RMSE: %.4f rad/s\n', rmse_r);

%% 车辆动力学模型函数
function x_next = vehicle_dynamics(x, u, m, Iz, lf, lr, Cf, Cr, dt)
    % 三自由度车辆模型动力学
    % 状态: x = [vx, vy, r]
    % 输入: u = [delta, Fx]
    
    vx = x(1);
    vy = x(2);
    r = x(3);
    
    delta = u(1);
    Fx = u(2);
    
    % 计算轮胎侧偏角
    alpha_f = atan2(vy + lf*r, vx) - delta;
    alpha_r = atan2(vy - lr*r, vx);
    
    % 计算轮胎侧向力
    Fyf = -Cf * alpha_f;
    Fyr = -Cr * alpha_r;
    
    % 动力学方程
    vx_dot = (Fx - Fyf*sin(delta))/m + vy*r;
    vy_dot = (Fyf*cos(delta) + Fyr)/m - vx*r;
    r_dot = (lf*Fyf*cos(delta) - lr*Fyr)/Iz;
    
    % 欧拉积分
    x_next = x + [vx_dot; vy_dot; r_dot] * dt;
end

%% 预测步骤函数
function [x_pred, F] = predict(x, u, m, Iz, lf, lr, Cf, Cr, dt)
    % 使用非线性模型进行状态预测
    % 并计算状态转移雅可比矩阵F
    
    vx = x(1);
    vy = x(2);
    r = x(3);
    
    delta = u(1);
    Fx = u(2);
    
    % 计算轮胎侧偏角
    alpha_f = atan2(vy + lf*r, vx) - delta;
    alpha_r = atan2(vy - lr*r, vx);
    
    % 计算轮胎侧向力
    Fyf = -Cf * alpha_f;
    Fyr = -Cr * alpha_r;
    
    % 动力学方程
    vx_dot = (Fx - Fyf*sin(delta))/m + vy*r;
    vy_dot = (Fyf*cos(delta) + Fyr)/m - vx*r;
    r_dot = (lf*Fyf*cos(delta) - lr*Fyr)/Iz;
    
    % 状态预测
    x_pred = x + [vx_dot; vy_dot; r_dot] * dt;
    
    % 计算状态转移雅可比矩阵F
    % 这里使用数值方法计算雅可比矩阵
    epsilon = 1e-6;
    F = zeros(3,3);
    
    for i = 1:3
        x_plus = x;
        x_plus(i) = x_plus(i) + epsilon;
        
        x_minus = x;
        x_minus(i) = x_minus(i) - epsilon;
        
        f_plus = vehicle_dynamics(x_plus, u, m, Iz, lf, lr, Cf, Cr, dt);
        f_minus = vehicle_dynamics(x_minus, u, m, Iz, lf, lr, Cf, Cr, dt);
        
        F(:, i) = (f_plus - f_minus) / (2 * epsilon);
    end
end

%% 测量模型函数
function z = measure(x)
    % 测量模型 - 假设我们可以直接测量所有状态
    % 在实际应用中，可能需要使用传感器模型
    z = x;
end

%% 测量模型雅可比矩阵函数
function [z_pred, H] = measurement_model(x)
    % 测量模型及其雅可比矩阵
    % 假设直接测量所有状态，所以H是单位矩阵
    z_pred = x;
    H = eye(3);
end