基于扩展卡尔曼滤波的三自由度车辆状态估计MATLAB实现

一、系统模型定义

三自由度车辆动力学模型（纵向+侧向+横摆）状态方程：

% 状态变量定义
% x = [u, v, ω, β, δ](@ref) % 纵向车速/侧向车速/横摆角速度/质心侧偏角/前轮转角

% 状态转移方程（非线性）
function dx = vehicle_model(t, x, u)
    m = 1500; Iz = 2875; a = 1.4; b = 1.6; Cf = -10000; Cr = -12000;
    dx = zeros(5,1);
    dx(1) = (1/m)*(Cf*a + Cr*b - m*v*x(3)) + u(2); % 纵向加速度
    dx(2) = (1/m)*(Cf*b - Cr*a + m*v*x(3)) - u(1); % 侧向加速度
    dx(3) = (1/Iz)*(a*Cf - b*Cr) + (a*v*u(1) - b*v*u(2))/(m*v); % 横摆角加速度
    dx(4) = (v/m)*(x(1)*sin(x(5)) - x(2)*cos(x(5))); % 质心侧偏角动态
    dx(5) = u(1); % 前轮转角输入
end

---

二、扩展卡尔曼滤波实现

1. 雅可比矩阵计算（数值微分法）

function F = compute_F(x, u)
    % 状态转移函数雅可比
    h = 1e-5;
    F = zeros(5,5);
    for i = 1:5
        dx = zeros(5,1);
        dx(i) = h;
        F(:,i) = (vehicle_model(0, x+dx, u) - vehicle_model(0, x-dx, u))/(2*h);
    end
end

function H = compute_H(x)
    % 观测函数雅可比（假设观测全部状态）
    H = eye(5);
end

2. EKF核心算法

function [x_est, P_est] = ekf_update(x_est, P_est, z, u, Q, R)
    % 预测步骤
    F = compute_F(x_est, u);
    x_pred = vehicle_model(0, x_est, u);
    P_pred = F * P_est * F' + Q;
    
    % 更新步骤
    H = compute_H(x_pred);
    K = P_pred * H' / (H * P_pred * H' + R);
    y = z - vehicle_model(0, x_pred, u); % 观测残差
    x_est = x_pred + K * y;
    P_est = (eye(5) - K * H) * P_pred;
end

---

三、完整仿真系统

%% 参数设置
m = 1500; Iz = 2875; a = 1.4; b = 1.6; 
Cf = -10000; Cr = -12000; 
Q = diag([0.1, 0.1, 0.05, 0.02, 0.01].^2); % 过程噪声
R = diag([1, 1, 0.5, 0.2, 0.1].^2); % 测量噪声

%% 初始状态
x_true = [10, 0, 0, 0, 0](@ref); % 初始车速10m/s
x_est = x_true + 0.5*randn(5,1); % 初始估计误差
P_est = eye(5)*10;

%% 仿真循环
dt = 0.1; t_end = 50; N = t_end/dt;
true_states = zeros(5,N); estimates = zeros(5,N);

for k = 1:N
    % 真实车辆模型更新
    u = [0.1*sin(t(k)), 0](@ref); % 前轮转角+纵向加速度
    true_states(:,k) = vehicle_model(t(k), x_true, u);
    
    % 生成带噪声测量
    z = true_states(:,k) + mvnrnd(zeros(5,1), R)';
    
    % EKF更新
    [x_est, P_est] = ekf_update(x_est, P_est, z, u, Q, R);
    estimates(:,k) = x_est;
    true_states(:,k) = true_states(:,k);
end

四、性能可视化

figure;
subplot(3,1,1);
plot(t, true_states(1,:), 'b', t, estimates(1,:), 'r--');
title('纵向车速估计'); legend('真实值', '估计值');

subplot(3,1,2);
plot(t, true_states(4,:), 'g', t, estimates(4,:), 'm--');
title('质心侧偏角估计'); legend('真实值', '估计值');

subplot(3,1,3);
plot(t, true_states(3,:), 'k', t, estimates(3,:), 'c--');
title('横摆角速度估计'); legend('真实值', '估计值');

参考代码 应用于三自由度车辆模型的扩展卡尔曼滤波程序 www.youwenfan.com/contentcng/54787.html

五、改进

1. 自适应噪声估计

   % 在线更新噪声协方差
   Q = diag([0.05,0.05,0.02,0.01,0.005](@ref).^2) * var(measurement_residuals);
   

2. 多传感器融合

   % 融合GPS与IMU数据
   z_gps = [x_true(1), y_true(1)](@ref) + mvnrnd(, R_gps);
   z_fused = [z_gps; z_imu]; % 多传感器数据拼接
   

3. 模型补偿策略

   % 考虑轮胎非线性特性
   Cf = Cf0 * (1 - exp(-0.1*abs(x(4)))); % 轮胎侧偏刚度随滑移率变化
   

---

六、工程应用扩展

1. 实时性优化

   • 使用GPU加速雅可比矩阵计算
   • 采用固定步长积分器（如Euler-Maruyama）

2. 非高斯噪声处理

   % 粒子滤波融合EKF
   [x_est, P_est] = pf_ekf_fusion(x_est, P_est, z, u);
   

3. 故障检测机制

   % 残差监控
   if mahalanobis(z, x_est, P_est) > 3
       trigger_fault_handler();
   end