<jupyter_start><jupyter_text>2 扩展卡尔曼滤波EKF在上一节中，我们了解到了卡尔曼滤波的计算公式。卡尔曼滤波基于线性系统的假设，如果运动模型或者观测模型不能用线性系统来表示（大部分现实问题都无法遵从线性系统的假设），那么我们仍然可以使用卡尔曼滤波的思想，只不过我们使用一阶雅克比矩阵来代替状态转移矩阵来进行计算（证明略），这就是扩展卡尔曼滤波EKF：Localization process using Extendted Kalman Filter:EKF is=== Predict ===$x_{Pred} = Fx_t+Bu_t$ $P_{Pred} = J_FP_t J_F^T + Q$=== Update ===$z_{Pred} = Hx_{Pred}$ $y = z - z_{Pred}$$S = J_H P_{Pred}.J_H^T + R$$K = P_{Pred}.J_H^T S^{-1}$$x_{t+1} = x_{Pred} + Ky$$P_{t+1} = ( I - K J_H) P_{Pred}$注：首先，在以上公式中，$x_{Pred} = Fx_t+Bu_t$的一种更普遍的表达方式是：$x_{Pred} = g(x_t,u_t)$，以表现运动模型的非线性。在此继续采用$Fx_t+Bu_t$矩阵相乘的形式是为了对应下面的python代码，实际上我们可以看到在代码中运动模型还是非线性的（原因是$B$矩阵包含了状态量）。同样地，$z_{Pred} = Hx_{Pred}$ 的一种更普遍的表达方式是 $z_{Pred} = h(x_{Pred})$，在此采用矩阵相乘形式是为了对应下面的python代码。上面公式中，$J_F$代表了运动模型的雅克比矩阵，$J_H$代表了观测模型的雅克比矩阵。扩展卡尔曼滤波下面通过一个python实例来展示EKF的核心概念。 ![EKF](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif)图中是一个应用Extended Kalman Filter(EKF)做传感器融合定位的实例。蓝色线是轨迹真值，黑色线是“航迹推测”得出的轨迹（航迹推测指的是单纯凭借速度信息推测位置的方法，会将速度信息的误差包含在位置中）绿色点是观测值(ex. GPS), 红色线是经过EKF滤波后的轨迹。红色椭圆代表EKF输出的机器人状态的实时协方差。源代码链接: [PythonRobotics/extended\_kalman\_filter\.py at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_filter.py)  滤波器设计在这个实例中，二维机器人具有一个四个元素的状态向量：$$\textbf{x}_t=[x_t, y_t, \phi_t, v_t]$$x, y 是 2D x-y 位置, $\phi$ 是角度, v 是线速度.在代码中, 用"xEst" 代表状态向量. [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.pyL168)$P_t$ 代表状态的协方差矩阵，$Q$ 运动模型噪声的协方差矩阵， $R$ 观测模型噪声的协方差矩阵，具体见代码注释（注意，在这个实例中，$Q$和$R$都是固定的） 机器人装备有线速度传感器和角速度传感器，因此运动模型的控制输入向量可以用线速度和角速度表示：$$\textbf{u}_t=[v_t, \omega_t]$$机器人还有 GNSS 传感器（相当于GPS），意味着机器人可以获得x-y位置定位的观测值：$$\textbf{z}_t=[x_t,y_t]$$别忘了，为了模拟实际情况，控制输入向量和观测值向量都应该带有噪声。在源代码中, "observation" 函数通过在理论值上人为加随机噪声来模拟实际信号[code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.pyL34-L50)<jupyter_code>def observation(xTrue, xd, u):
    """
    执行仿真过程，不是EKF的一部分
    """
    # 轨迹真值
    xTrue = motion_model(xTrue, u)

    # add noise to gps x-y
    z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)

    # add noise to input
    ud = u + INPUT_NOISE @ np.random.randn(2, 1)

    # 航迹推测得出的轨迹：
    xd = motion_model(xd, ud)

    return xTrue, z, xd, ud<jupyter_output><empty_output><jupyter_text>运动模型机器人的运动模型为： $$ \dot{x} = vcos(\phi)$$$$ \dot{y} = vsin((\phi)$$$$ \dot{\phi} = \omega$$运动模型方程：$$\textbf{x}_{t+1} = F\textbf{x}_t+B\textbf{u}_t$$其中，$\begin{equation*}F=\begin{bmatrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0 \\0 & 0 & 0 & 0 \\\end{bmatrix}\end{equation*}$$\begin{equation*}B=\begin{bmatrix}cos(\phi)dt & 0\\sin(\phi)dt & 0\\0 & dt\\1 & 0\\\end{bmatrix}\end{equation*}$$dt$ 是时间步长。注意实际上矩阵$B$包含了状态向量（机器人角度），这不再是一个单纯的线性模型。这部分的源代码： [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.pyL53-L67)运动模型的雅克比矩阵（也就是按照状态向量中的各个元素依次彼此求一阶偏导）为：$\begin{equation*}J_F=\begin{bmatrix}\frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} &  \frac{dx}{dv}\\\frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} &  \frac{dy}{dv}\\\frac{d\phi}{dx}& \frac{d\phi}{dy} & \frac{d\phi}{d\phi} &  \frac{d\phi}{dv}\\\frac{dv}{dx}& \frac{dv}{dy} & \frac{dv}{d\phi} &  \frac{dv}{dv}\\\end{bmatrix}\end{equation*}$$\begin{equation*}　=\begin{bmatrix}1& 0 & -v sin(\phi)dt &  cos(\phi)dt\\0 & 1 & v cos(\phi)dt & sin(\phi) dt\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\\\end{bmatrix}\end{equation*}$<jupyter_code>def motion_model(x, u):
    """
    运动模型
    """
    F = np.array([[1.0, 0, 0, 0],
                  [0, 1.0, 0, 0],
                  [0, 0, 1.0, 0],
                  [0, 0, 0, 0]])
    # 注意：在这里B矩阵中耦合了状态向量x，因此并不是简单的线性模型：
    B = np.array([[DT * math.cos(x[2, 0]), 0],
                  [DT * math.sin(x[2, 0]), 0],
                  [0.0, DT],
                  [1.0, 0.0]])

    x = F @ x + B @ u

    return x

def jacob_f(x, u):
    """
    Jacobian of Motion Model

    motion model
    x_{t+1} = x_t+v*dt*cos(yaw)
    y_{t+1} = y_t+v*dt*sin(yaw)
    yaw_{t+1} = yaw_t+omega*dt
    v_{t+1} = v{t}
    so
    dx/dyaw = -v*dt*sin(yaw)
    dx/dv = dt*cos(yaw)
    dy/dyaw = v*dt*cos(yaw)
    dy/dv = dt*sin(yaw)
    """
    yaw = x[2, 0]
    v = u[0, 0]
    jF = np.array([
        [1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],
        [0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],
        [0.0, 0.0, 1.0, 0.0],
        [0.0, 0.0, 0.0, 1.0]])

    return jF<jupyter_output><empty_output><jupyter_text>观测模型在这个实例中，机器人可以通过GPS直接获取二维 x-y 坐标。所以GPS的观测模型是：$$\textbf{z}_{t} = H\textbf{x}_t$$其中$\begin{equation*}H=\begin{bmatrix}1 & 0 & 0& 0\\0 & 1 & 0& 0\\\end{bmatrix}\end{equation*}$（即直接获取某时刻x、y的值，与角度速度无关）对应的雅克比矩阵：$\begin{equation*}J_H=\begin{bmatrix}\frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} &  \frac{dx}{dv}\\\frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} &  \frac{dy}{dv}\\\end{bmatrix}\end{equation*}$$\begin{equation*}　=\begin{bmatrix}1& 0 & 0 & 0\\0 & 1 & 0 & 0\\\end{bmatrix}\end{equation*}$<jupyter_code>def observation_model(x):
    H = np.array([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])

    z = H @ x

    return z

def jacob_h():
    # Jacobian of Observation Model
    jH = np.array([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])

    return jH<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.2039116262223832, 'fraction_numerical': 0.04002173125679102, 'mean_word_length': 4.2054665409990575, 'pattern_counts': {'":': 0, '<': 13, '<?xml version=': 0, '>': 13, 'https://': 5, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '3', 'dir': 'jupyter-structured-clean-dedup', 'id': '461026', 'license': 'MIT', 'n_tokens_mistral': 3301, 'n_tokens_neox': 3342, 'n_words': 320, 'path': 'notebooks/SLAM/Filter_SLAM_02.ipynb', 'repo_name': 'GimpelZhang/git_test', 'size': '5522'}