车辆动力学参数估算：EKF与UKF的探索

UKF无迹卡尔曼滤波、EKF扩展卡尔曼滤波估算路面附着系数模型 三自由度车辆模型估算纵向车速、横摆角速度和质心侧偏角 本在车辆动力学研究领域准确估算路面附着系数、纵向车速、横摆角速度和质心侧偏角等关键参数至关重要。今天咱们就来唠唠 EKF扩展卡尔曼滤波和 UKF无迹卡尔曼滤波在这方面的应用特别是基于三自由度车辆模型的相关估算。三自由度车辆模型基础三自由度车辆模型主要考虑车辆纵向、侧向以及横摆三个方向的运动。通过建立相应的动力学方程来描述车辆在这三个方向上的运动状态变化。例如纵向动力学方程可以简单表示为# 假设 m 是车辆质量F_x 是纵向力v_x 是纵向车速 # 纵向动力学方程的简单 Python 代码示意 m 1500 # 车辆质量单位 kg F_x 5000 # 纵向力单位 N v_x_dot F_x / m # 纵向加速度单位 m/s^2这段代码体现了牛顿第二定律在车辆纵向动力学上的应用通过已知的纵向力和车辆质量计算纵向加速度。同理侧向和横摆方向也有类似的动力学方程它们共同构成了三自由度车辆模型的基础动力学描述。EKF 扩展卡尔曼滤波估算EKF 是在传统卡尔曼滤波基础上发展而来主要用于处理非线性系统。在车辆参数估算中我们面对的车辆模型往往是非线性的这就凸显了 EKF 的价值。EKF 原理简述EKF 的核心思想是通过对非线性函数进行一阶泰勒展开线性化然后套用卡尔曼滤波的框架进行状态估计。代码实现示例简化示意import numpy as np # 定义状态转移矩阵 F F np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) # 定义观测矩阵 H H np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) # 初始化状态估计 x_hat x_hat np.array([[0], [0], [0], [0]]) # 初始化协方差矩阵 P P np.eye(4) # 过程噪声协方差 Q Q np.diag([0.01, 0.01, 0.01, 0.01]) # 观测噪声协方差 R R np.diag([0.1, 0.1]) # 假设有观测值 z z np.array([[10], [0.1]]) # 预测步骤 x_hat_minus F.dot(x_hat) P_minus F.dot(P).dot(F.T) Q # 更新步骤 K P_minus.dot(H.T).dot(np.linalg.inv(H.dot(P_minus).dot(H.T) R)) x_hat x_hat_minus K.dot(z - H.dot(x_hat_minus)) P (np.eye(4) - K.dot(H)).dot(P_minus)代码分析在这段代码中我们首先定义了状态转移矩阵F和观测矩阵H它们分别描述了状态如何随时间转移以及状态与观测之间的关系。然后初始化状态估计x_hat和协方差矩阵P。在预测步骤中我们根据状态转移矩阵预测下一时刻的状态和协方差。在更新步骤中通过卡尔曼增益K将预测值与观测值相结合得到更准确的状态估计。UKF 无迹卡尔曼滤波估算UKF 也是用于非线性系统的滤波方法但它与 EKF 的线性化方式不同。UKF 采用无迹变换UT来处理非线性问题相比 EKF 通常能提供更准确的估计。UKF 原理简述UKF 首先通过无迹变换生成一组 sigma 点这些点能够较好地捕捉非线性函数的统计特性。然后通过这些 sigma 点的传播和加权计算来更新状态估计和协方差。代码实现示例简化示意import numpy as np # 定义状态向量 x x np.array([[0], [0], [0], [0]]) # 定义协方差矩阵 P P np.eye(4) # 定义过程噪声协方差 Q Q np.diag([0.01, 0.01, 0.01, 0.01]) # 定义观测噪声协方差 R R np.diag([0.1, 0.1]) # 计算 sigma 点 n x.shape[0] lambda_ 3 - n W_m np.zeros((2 * n 1, 1)) W_c np.zeros((2 * n 1, 1)) W_m[0] lambda_ / (lambda_ n) W_c[0] lambda_ / (lambda_ n) (1 - 1 / (lambda_ n) 0) for i in range(1, 2 * n 1): W_m[i] 1 / (2 * (lambda_ n)) W_c[i] 1 / (2 * (lambda_ n)) sigma_points np.zeros((n, 2 * n 1)) sigma_points[:, 0] x.flatten() for i in range(1, n 1): sigma_points[:, i] (x np.sqrt((lambda_ n) * P)[:, i - 1]).flatten() sigma_points[:, i n] (x - np.sqrt((lambda_ n) * P)[:, i - 1]).flatten() # 预测步骤 x_hat_minus np.zeros((n, 1)) P_minus np.zeros((n, n)) for i in range(2 * n 1): x_hat_minus x_hat_minus W_m[i] * np.array([[sigma_points[0, i]], [sigma_points[1, i]], [sigma_points[2, i]], [sigma_points[3, i]]]) for i in range(2 * n 1): P_minus P_minus W_c[i] * (np.array([[sigma_points[0, i]], [sigma_points[1, i]], [sigma_points[2, i]], [sigma_points[3, i]]]) - x_hat_minus).dot((np.array([[sigma_points[0, i]], [sigma_points[1, i]], [sigma_points[2, i]], [sigma_points[3, i]]]) - x_hat_minus).T) P_minus P_minus Q # 假设有观测值 z z np.array([[10], [0.1]]) # 计算观测的 sigma 点 z_sigma_points np.zeros((2, 2 * n 1)) for i in range(2 * n 1): z_sigma_points[:, i] h(sigma_points[:, i]) # h 是观测函数 # 更新步骤 z_hat np.zeros((2, 1)) for i in range(2 * n 1): z_hat z_hat W_m[i] * np.array([[z_sigma_points[0, i]], [z_sigma_points[1, i]]]) P_zz np.zeros((2, 2)) P_xz np.zeros((n, 2)) for i in range(2 * n 1): P_zz P_zz W_c[i] * (z_sigma_points[:, i].reshape(-1, 1) - z_hat).dot((z_sigma_points[:, i].reshape(-1, 1) - z_hat).T) P_xz P_xz W_c[i] * (sigma_points[:, i].reshape(-1, 1) - x_hat_minus).dot((z_sigma_points[:, i].reshape(-1, 1) - z_hat).T) P_zz P_zz R K P_xz.dot(np.linalg.inv(P_zz)) x_hat x_hat_minus K.dot(z - z_hat) P P_minus - K.dot(P_zz).dot(K.T)代码分析在 UKF 的代码实现中首先计算了 sigma 点及其权重。预测步骤通过对 sigma 点的加权计算得到预测状态和协方差。更新步骤同样利用了 sigma 点的传播来计算观测预测值以及相关协方差进而得到卡尔曼增益并更新状态估计和协方差。总结EKF 和 UKF 在基于三自由度车辆模型估算路面附着系数、纵向车速等参数方面各有优劣。EKF 计算相对简单但线性化过程可能引入误差UKF 能更好处理非线性但计算量较大。在实际应用中需要根据具体需求和系统特性来选择合适的滤波方法以实现车辆动力学参数的准确估算。希望今天的分享能让大家对这两种滤波方法在车辆领域的应用有更清晰的认识。UKF无迹卡尔曼滤波、EKF扩展卡尔曼滤波估算路面附着系数模型 三自由度车辆模型估算纵向车速、横摆角速度和质心侧偏角 本