车辆状态估计，扩展卡尔曼滤波EKF，无迹卡尔曼滤波UKF车辆状态估计，扩展卡尔曼滤波EKF，无迹卡尔曼滤波UKF 角阶跃输入+整车7自由度模型+UKF状态估计模型+附送EKF状态估计模型，针对于轮毂电机分布式驱动车辆，进行车速，质心侧偏角，横摆角速度估计。 模型输入：方向盘转角delta，车辆纵向加速度ax 模型输出：横摆角速度wz，纵向车速vx，质心侧偏角β

在车辆动力学研究中，准确估计车辆状态对于车辆的稳定性控制和智能驾驶等方面至关重要。今天咱就唠唠基于角阶跃输入、整车 7 自由度模型，用扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）对轮毂电机分布式驱动车辆进行车速、质心侧偏角和横摆角速度估计这事儿。

1. 整车 7 自由度模型

整车 7 自由度模型考虑了车辆的纵向、侧向、垂向运动以及横摆、侧倾、俯仰运动，这里咱聚焦跟状态估计相关的侧向和横摆运动部分。通过一系列复杂的力学推导（具体公式就不展开折磨大家了，不然篇幅太长），我们可以得到描述车辆运动状态的状态空间方程。

假设车辆状态向量 $X = [vx, \beta, \omegaz]^T$，分别代表纵向车速、质心侧偏角和横摆角速度。输入向量 $U = [\delta, a_x]^T$，即方向盘转角和车辆纵向加速度。

状态方程可大致写成：$\dot{X} = f(X, U)$

这里的 $f$ 函数包含了车辆动力学相关的各种参数，像车辆质量、转动惯量、轮胎侧偏刚度等等。

2. 扩展卡尔曼滤波（EKF）

2.1 原理

EKF 是卡尔曼滤波在非线性系统中的扩展。它通过对非线性函数进行一阶泰勒展开来线性化。对于我们的车辆状态估计问题，预测步骤和更新步骤跟标准卡尔曼滤波类似，但有一些非线性的处理。

2.2 代码示例（Python 伪代码）

import numpy as np # 假设已经定义了状态转移函数 f 和观测函数 h def ekf_predict(X, P, Q, dt, U): # 预测状态 X = f(X, U, dt) # 计算状态转移矩阵 F F = np.eye(3) # 预测协方差 P = F @ P @ F.T + Q return X, P def ekf_update(X, P, Z, R, U): # 计算观测矩阵 H H = np.eye(3) # 计算卡尔曼增益 K S = H @ P @ H.T + R K = P @ H.T @ np.linalg.inv(S) # 更新状态 X = X + K @ (Z - h(X, U)) # 更新协方差 P = (np.eye(3) - K @ H) @ P return X, P

2.3 代码分析

ekfpredict函数里，先通过状态转移函数f预测下一时刻状态X，然后手动设置一个简单的状态转移矩阵F（实际应用需根据具体模型计算），接着根据公式更新预测协方差P。ekfupdate函数中，计算观测矩阵H（同样这里是简单示意），通过协方差相关计算得到卡尔曼增益K，利用观测值Z更新状态X和协方差P。

3. 无迹卡尔曼滤波（UKF）

3.1 原理

UKF 不像 EKF 那样对非线性函数线性化，而是通过 Sigma 点采样来近似概率分布。它能更准确地处理非线性问题，对于车辆这种非线性系统很适用。

3.2 代码示例（Python 伪代码）

def ukf_predict(X, P, Q, dt, U): # 计算 Sigma 点 n = X.shape[0] lambda_ = 3 - n Wm = np.zeros(2 * n + 1) Wc = np.zeros(2 * n + 1) Wm[0] = lambda_ / (lambda_ + n) Wc[0] = lambda_ / (lambda_ + n) + (1 - np.power(2, -n)) for i in range(1, 2 * n + 1): Wm[i] = 1 / (2 * (lambda_ + n)) Wc[i] = 1 / (2 * (lambda_ + n)) X_sigma = np.zeros((n, 2 * n + 1)) X_sigma[:, 0] = X for i in range(n): X_sigma[:, i + 1] = X + np.sqrt((lambda_ + n) * P)[i, :] X_sigma[:, i + n + 1] = X - np.sqrt((lambda_ + n) * P)[i, :] # 预测 Sigma 点 X_sigma_pred = np.zeros((n, 2 * n + 1)) for i in range(2 * n + 1): X_sigma_pred[:, i] = f(X_sigma[:, i], U, dt) # 预测状态 X = np.sum(Wm * X_sigma_pred, axis = 1).reshape(-1, 1) # 预测协方差 P = np.zeros((n, n)) for i in range(2 * n + 1): P += Wc[i] * (X_sigma_pred[:, i].reshape(-1, 1) - X) @ (X_sigma_pred[:, i].reshape(-1, 1) - X).T P += Q return X, P def ukf_update(X, P, Z, R, U): # 计算 Sigma 点 n = X.shape[0] lambda_ = 3 - n Wm = np.zeros(2 * n + 1) Wc = np.zeros(2 * n + 1) Wm[0] = lambda_ / (lambda_ + n) Wc[0] = lambda_ / (lambda_ + n) + (1 - np.power(2, -n)) for i in range(1, 2 * n + 1): Wm[i] = 1 / (2 * (lambda_ + n)) Wc[i] = 1 / (2 * (lambda_ + n)) X_sigma = np.zeros((n, 2 * n + 1)) X_sigma[:, 0] = X for i in range(n): X_sigma[:, i + 1] = X + np.sqrt((lambda_ + n) * P)[i, :] X_sigma[:, i + n + 1] = X - np.sqrt((lambda_ + n) * P)[i, :] # 预测 Sigma 点 Z_sigma = np.zeros((n, 2 * n + 1)) for i in range(2 * n + 1): Z_sigma[:, i] = h(X_sigma[:, i], U) # 预测观测值 Z_pred = np.sum(Wm * Z_sigma, axis = 1).reshape(-1, 1) # 计算协方差 Pzz = np.zeros((n, n)) Pxz = np.zeros((n, n)) for i in range(2 * n + 1): Pzz += Wc[i] * (Z_sigma[:, i].reshape(-1, 1) - Z_pred) @ (Z_sigma[:, i].reshape(-1, 1) - Z_pred).T Pxz += Wc[i] * (X_sigma[:, i].reshape(-1, 1) - X) @ (Z_sigma[:, i].reshape(-1, 1) - Z_pred).T Pzz += R # 计算卡尔曼增益 K K = Pxz @ np.linalg.inv(Pzz) # 更新状态 X = X + K @ (Z - Z_pred) # 更新协方差 P = P - K @ Pzz @ K.T return X, P

3.3 代码分析

ukfpredict函数里，先计算 Sigma 点相关的权重Wm和Wc以及 Sigma 点Xsigma，通过状态转移函数f预测 Sigma 点Xsigmapred，然后根据权重计算预测状态X和预测协方差P。ukfupdate函数类似，计算观测相关的 Sigma 点Zsigma，预测观测值Z_pred，通过协方差计算得到卡尔曼增益K来更新状态X和协方差P。

4. 总结

通过角阶跃输入结合整车 7 自由度模型，利用 EKF 和 UKF 可以有效地对轮毂电机分布式驱动车辆的车速、质心侧偏角和横摆角速度进行估计。UKF 在处理非线性方面有优势，但计算量相对 EKF 大些。实际应用中可根据具体需求和硬件条件选择合适的滤波方法。希望这些内容能给搞车辆动力学和状态估计的小伙伴们一些启发。