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

在车辆动力学控制领域，准确估计车辆状态至关重要。今天咱就聊聊基于角阶跃输入、整车7自由度模型，搭配UKF和EKF状态估计模型，对轮毂电机分布式驱动车辆的车速、质心侧偏角和横摆角速度进行估计的事儿。

一、模型输入与输出

咱的模型输入主要是方向盘转角 delta 和车辆纵向加速度 ax 。而输出则是横摆角速度 wz 、纵向车速 vx 以及质心侧偏角 β 。

二、整车7自由度模型

整车7自由度模型是整个估计过程的基础，它考虑了车辆的纵向、侧向、垂向运动以及横摆、侧倾、俯仰运动，能较为全面地描述车辆的动力学特性。不过这里咱不展开讲模型细节，重点放在状态估计上。

三、扩展卡尔曼滤波（EKF）

EKF是将非线性系统在当前估计值附近进行线性化，然后应用卡尔曼滤波框架。下面简单用Python代码示意下EKF部分关键步骤（这里为简化示意，非完整可运行代码）：

import numpy as np

# 假设的状态转移函数 f(x, u)，这里x是状态，u是输入
def f(x, u, dt):
    F = np.array([[1, 0, 0],
                  [0, 1, 0],
                  [0, 0, 1]])
    B = np.array([[dt, 0],
                  [0, dt],
                  [0, 0]])
    return F.dot(x) + B.dot(u)

# 假设的观测函数 h(x)
def h(x):
    return np.array([x[0], x[1], x[2]])

# 初始化状态和协方差
x_hat = np.array([0, 0, 0]).reshape(-1, 1)  # 初始状态估计
P = np.eye(3)  # 初始协方差矩阵
Q = np.diag([0.01, 0.01, 0.01])  # 过程噪声协方差
R = np.diag([0.1, 0.1, 0.1])  # 观测噪声协方差

# 预测步骤
def predict(x_hat, P, u, dt):
    x_hat = f(x_hat, u, dt)
    F = np.array([[1, 0, 0],
                  [0, 1, 0],
                  [0, 0, 1]])
    P = F.dot(P).dot(F.T) + Q
    return x_hat, P

# 更新步骤
def update(x_hat, P, z):
    H = np.array([[1, 0, 0],
                  [0, 1, 0],
                  [0, 0, 1]])
    y = z - h(x_hat)
    S = H.dot(P).dot(H.T) + R
    K = P.dot(H.T).dot(np.linalg.inv(S))
    x_hat = x_hat + K.dot(y)
    P = (np.eye(3) - K.dot(H)).dot(P)
    return x_hat, P

分析一下上面代码哈，f 函数就是状态转移函数，根据车辆动力学，它描述了状态如何随时间和输入变化，这里简单用一个线性近似来表示。h 函数是观测函数，告诉我们怎么从状态得到观测值。predict 步骤根据上一时刻的状态和输入预测当前状态和协方差，update 步骤则结合实际观测值来修正预测结果，通过卡尔曼增益 K 平衡预测和观测的权重。

四、无迹卡尔曼滤波（UKF）

UKF则是通过采样点来近似非线性系统的概率分布，避免了EKF的线性化误差。同样用Python代码简单示意关键部分（同样非完整代码）：

def unscented_transform(x_hat, P):
    n = x_hat.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 - 1 / (lambda_ + n) + 0)
    for i in range(1, 2 * n + 1):
        Wm[i] = 1 / (2 * (lambda_ + n))
        Wc[i] = 1 / (2 * (lambda_ + n))

    Sigma = np.linalg.cholesky((lambda_ + n) * P)
    Xsig = np.zeros((n, 2 * n + 1))
    Xsig[:, 0] = x_hat.flatten()
    for i in range(n):
        Xsig[:, i + 1] = (x_hat + Sigma[:, i]).flatten()
        Xsig[:, i + n + 1] = (x_hat - Sigma[:, i]).flatten()
    return Xsig, Wm, Wc

# UKF预测步骤
def ukf_predict(x_hat, P, u, dt):
    Xsig, Wm, Wc = unscented_transform(x_hat, P)
    n = x_hat.shape[0]
    x_hat = np.zeros((n, 1))
    P = np.zeros((n, n))
    for i in range(2 * n + 1):
        x_hat = x_hat + Wm[i] * f(Xsig[:, i].reshape(-1, 1), u, dt)
    for i in range(2 * n + 1):
        x_diff = f(Xsig[:, i].reshape(-1, 1), u, dt) - x_hat
        P = P + Wc[i] * x_diff.dot(x_diff.T)
    P = P + Q
    return x_hat, P

# UKF更新步骤
def ukf_update(x_hat, P, z):
    Xsig, Wm, Wc = unscented_transform(x_hat, P)
    n = x_hat.shape[0]
    z_hat = np.zeros((n, 1))
    Pzz = np.zeros((n, n))
    Pxz = np.zeros((n, n))
    for i in range(2 * n + 1):
        z_hat = z_hat + Wm[i] * h(Xsig[:, i].reshape(-1, 1))
    for i in range(2 * n + 1):
        z_diff = h(Xsig[:, i].reshape(-1, 1)) - z_hat
        x_diff = Xsig[:, i].reshape(-1, 1) - x_hat
        Pzz = Pzz + Wc[i] * z_diff.dot(z_diff.T)
        Pxz = Pxz + Wc[i] * x_diff.dot(z_diff.T)
    Pzz = Pzz + R
    K = Pxz.dot(np.linalg.inv(Pzz))
    y = z - z_hat
    x_hat = x_hat + K.dot(y)
    P = P - K.dot(Pzz).dot(K.T)
    return x_hat, P

在UKF里，unscentedtransform 函数通过计算Sigma点来近似状态分布，ukfpredict 和 ukf_update 分别完成预测和更新步骤。与EKF不同，UKF通过这些Sigma点更准确地捕捉非线性系统的特性，减少线性化带来的误差。

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

通过角阶跃输入、整车7自由度模型以及EKF和UKF状态估计模型的结合，我们能够较为准确地对轮毂电机分布式驱动车辆的关键状态进行估计，为车辆的稳定性控制等高级应用奠定基础。