基于平方根容积卡尔曼/无迹卡尔曼 实现纵向、侧向速度、横摆角速度以及质心侧偏角的估计 轮胎模型:dugoff 算法:SRCKF/UKF Carsim/Simulink联仿 有m语言仿真模型，也实现了C程序仿真模型 以下为C程序仿真模型对质心侧偏角估计效果图 分别为:低附 高附 对开 对接(双移线工况)

在车辆动力学研究领域，准确估计纵向、侧向速度、横摆角速度以及质心侧偏角等关键状态参数至关重要。本文将探讨如何基于平方根容积卡尔曼（SRCKF）和无迹卡尔曼（UKF）算法，利用 Dugoff 轮胎模型，通过 Carsim 与 Simulink 联合仿真，并实现 M 语言和 C 程序仿真模型。

轮胎模型 - Dugoff

Dugoff 轮胎模型是车辆动力学仿真中常用的模型，它综合考虑了轮胎的纵向力和侧向力与滑移率、侧偏角之间的关系。以下是简化的 Dugoff 轮胎模型在代码中的体现：

// 假设已经定义了相关的变量，如侧偏角 alpha，纵向滑移率 s，垂直载荷 Fz 等
// 计算轮胎侧偏刚度
double C_alpha = getCAlpha(); 
// 根据 Dugoff 模型计算侧向力 Fy
double Fy = C_alpha * alpha * (1 - (fabs(C_alpha * alpha) / (mu * Fz))); 

在这段代码中，首先获取轮胎侧偏刚度 C_alpha，然后根据 Dugoff 模型计算侧向力 Fy。这里用到了侧偏角 alpha、垂直载荷 Fz 以及摩擦系数 mu，通过一定的公式关系得到侧向力，为后续车辆动力学计算提供基础。

算法 - SRCKF/UKF

平方根容积卡尔曼滤波（SRCKF）

SRCKF 是一种基于容积准则的卡尔曼滤波改进算法，它在处理非线性系统时具有较高的精度和稳定性。以下是 SRCKF 算法核心部分的伪代码：

% 初始化
P = chol(P); % P 是协方差矩阵，chol 函数进行 Cholesky 分解
x_hat = zeros(n, 1); % n 是状态向量维度，初始化状态估计值

% 预测步骤
[X_sigma] = SigmaPoints(x_hat, P); % 计算 Sigma 点
X_sigma = f(X_sigma); % 通过系统模型传递 Sigma 点
x_hat_minus = sum(W_m.* X_sigma, 2); % 计算预测状态
P_minus = zeros(n);
for k = 1:2*n+1
    P_minus = P_minus + W_c(k) * (X_sigma(:,k) - x_hat_minus) * (X_sigma(:,k) - x_hat_minus)';
end
P_minus = cholupdate(P_minus, R, 'L'); % 加入过程噪声协方差 R

% 更新步骤
[Y_sigma] = SigmaPoints(x_hat_minus, P_minus);
Y_sigma = h(Y_sigma); % 通过观测模型传递 Sigma 点
y_hat = sum(W_m.* Y_sigma, 2); % 计算预测观测值
S = zeros(m);
for k = 1:2*n+1
    S = S + W_c(k) * (Y_sigma(:,k) - y_hat) * (Y_sigma(:,k) - y_hat)';
end
S = cholupdate(S, Q, 'L'); % 加入观测噪声协方差 Q
K = (P_minus * (Y_sigma - repmat(y_hat, 1, 2*n+1)) * W_c) / S; % 计算卡尔曼增益
x_hat = x_hat_minus + K * (y - y_hat); % 更新状态估计值
P = cholupdate(P_minus, -K * S, 'L'); % 更新协方差矩阵

在 SRCKF 预测步骤中，首先计算 Sigma 点，通过系统模型传递后得到预测状态和预测协方差矩阵，并考虑过程噪声。更新步骤中，通过观测模型传递 Sigma 点得到预测观测值，计算卡尔曼增益后更新状态估计值和协方差矩阵，以此不断修正估计结果。

无迹卡尔曼滤波（UKF）

UKF 同样是处理非线性系统的有效算法，它通过选择一组 sigma 点来近似状态分布。以下是 UKF 的核心伪代码：

% 初始化
x_hat = zeros(n, 1);
P = eye(n);
% 计算 sigma 点权重
lambda = alpha^2 * (n + kappa) - n;
W_m = [lambda / (n + lambda), 0.5 / (n + lambda) * ones(1, 2*n)];
W_c = W_m;
W_c(1) = W_c(1) + (1 - alpha^2 + beta);

% 预测步骤
X_sigma = SigmaPoints(x_hat, P, lambda);
X_sigma = f(X_sigma);
x_hat_minus = sum(W_m.* X_sigma, 2);
P_minus = zeros(n);
for k = 1:2*n+1
    P_minus = P_minus + W_c(k) * (X_sigma(:,k) - x_hat_minus) * (X_sigma(:,k) - x_hat_minus)';
end
P_minus = P_minus + Q;

% 更新步骤
Y_sigma = SigmaPoints(x_hat_minus, P_minus, lambda);
Y_sigma = h(Y_sigma);
y_hat = sum(W_m.* Y_sigma, 2);
S = zeros(m);
for k = 1:2*n+1
    S = S + W_c(k) * (Y_sigma(:,k) - y_hat) * (Y_sigma(:,k) - y_hat)';
end
S = S + R;
T = zeros(n, m);
for k = 1:2*n+1
    T = T + W_c(k) * (X_sigma(:,k) - x_hat_minus) * (Y_sigma(:,k) - y_hat)';
end
K = T / S;
x_hat = x_hat_minus + K * (y - y_hat);
P = P_minus - K * S * K';

UKF 与 SRCKF 类似，在初始化后进行预测和更新步骤。不同之处在于权重计算以及协方差更新方式等细节，UKF 通过特定的权重分配和协方差计算方式来处理非线性问题。

Carsim/Simulink 联仿

通过 Carsim 与 Simulink 联合仿真，可以充分利用 Carsim 精确的车辆动力学模型和 Simulink 强大的控制算法设计与仿真能力。在联仿中，将车辆的物理模型搭建在 Carsim 中，而状态估计的算法，如 SRCKF 或 UKF 则在 Simulink 中实现。通过接口传递数据，实现车辆状态的实时估计和反馈。

仿真模型实现

M 语言仿真模型

在 M 语言（Matlab）中，可以方便地实现上述算法和模型。通过编写函数和脚本，构建完整的车辆状态估计仿真流程。例如，定义系统模型函数 f 和观测模型函数 h，结合 SRCKF 或 UKF 算法代码，实现对纵向、侧向速度、横摆角速度以及质心侧偏角的估计。

C 程序仿真模型

在 C 程序中实现仿真模型，可以提高运行效率，便于在实际车辆控制系统中应用。以下是 C 程序仿真模型对质心侧偏角估计效果图，分别展示了低附、高附、对开、对接（双移线工况）下的情况。

!低附工况质心侧偏角估计效果图

!高附工况质心侧偏角估计效果图

基于平方根容积卡尔曼/无迹卡尔曼 实现纵向、侧向速度、横摆角速度以及质心侧偏角的估计 轮胎模型:dugoff 算法:SRCKF/UKF Carsim/Simulink联仿 有m语言仿真模型，也实现了C程序仿真模型 以下为C程序仿真模型对质心侧偏角估计效果图 分别为:低附 高附 对开 对接(双移线工况)

!对开工况质心侧偏角估计效果图

!对接工况质心侧偏角估计效果图

从这些效果图可以看出，基于 SRCKF/UKF 算法的 C 程序仿真模型在不同路面附着条件和工况下，都能够较好地估计质心侧偏角，为车辆动力学控制提供了可靠的状态参数估计。

通过上述从轮胎模型到算法实现，再到联合仿真和不同语言仿真模型的构建，我们全面地实现了基于平方根容积卡尔曼/无迹卡尔曼的车辆关键状态参数估计，为车辆主动安全控制系统的开发提供了有力的技术支持。