深蓝学院自主泊车第2次作业-EKF

1 题目

给定二维小车，其状态定义为$\mathbf{x}=(x,y,\theta )$，$x$表示二维运动中的横坐标值，$y$表示二维运行中的纵坐标值，$\theta$表示运行的方向，也叫作航向。控制输入定义为$\mathbf{u}=(v,\omega )$，其中$v$表示小车的速度，它是一个标量，它的标准差为${\sigma }_v$；$\omega$是小车的角速度，它也是一个标量，它的标准差为${\sigma }_{\omega }$。那么小车的运动方程可写为，

$$\begin{array}{rl}{x}_k& =x_{k-1}+v_k\cdot cos({\theta }_{k-1})\cdot \Delta t\\ y_k& =y_{k-1}+v_k\cdot sin({\theta }_{k-1})\cdot \Delta t\\ {\theta }_k& ={\theta }_{k-1}+{\omega }_k\cdot \Delta t\end{array}\text{\hspace{1em}(1)}$$
整体可写为，${\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{w}}_k$，显然$\mathbf{f}(\cdot )$是一个非线性函数。

同样的，小车的测量方程为，
$${\mathbf{z}}_k=\mathbf{h}({\mathbf{x}}_k)+{\mathbf{v}}_k={\mathbf{x}}_k+{\mathbf{v}}_k\text{\hspace{1em}(2)}$$
它是一个线性函数。${\mathbf{v}}_k$是一个$3\times 1$维的向量，分别表示$x,y,\theta$上的噪声，它们的标准差分别为${\sigma }_x,{\sigma }_y,{\sigma }_{\theta }$。

$(v,\omega )$的数据频率为100Hz，且${\sigma }_v,{\sigma }_{\omega }$已知。$\mathbf{z}$的数据频率为10Hz，且${\sigma }_x,{\sigma }_y,{\sigma }_{\theta }$已知。使用扩展卡尔曼滤波器估计小车的状态$(x,y,\theta )$。

2 求解

依据扩展卡尔曼的那5个公式，可以得到如下公式，

预测部分：

$${\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k)\text{\hspace{1em}(3)}$$
$${\mathbf{P}}_k={\mathbf{A}}_k{\mathbf{P}}_{k-1}{\mathbf{A}}_k^T+{\mathbf{B}}_k{\mathbf{R}}_k{\mathbf{B}}_k^T\text{\hspace{1em}(4)}$$
其中，
$${\mathbf{A}}_k=\left[\begin{array}{ccc}1& 0& -v_k\cdot sin{\theta }_{k-1}\cdot \Delta t\\ 0& 1& v_k\cdot cos{\theta }_{k-1}\cdot \Delta t\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(5)}$$
$${\mathbf{B}}_k=\left[\begin{array}{ccc}cos{\theta }_{k-1}\cdot \Delta t& 0& \\ sin{\theta }_{k-1}\cdot \Delta t& 0& \\ 0& & \Delta t\end{array}\right]\text{\hspace{1em}(6)}$$
$${\mathbf{R}}_k=\left[\begin{array}{c}{\sigma }_v^2\\ {\sigma }_{\omega }^2\end{array}\right]\text{\hspace{1em}(7)}$$

更新部分：
$${\mathbf{K}}_k={\mathbf{P}}_{k,pred}({\mathbf{P}}_{k,pred}+{\mathbf{Q}}_k{)}^{-1}\text{\hspace{1em}(8)}$$
$${\mathbf{x}}_k={\mathbf{x}}_{k,pred}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{x}}_{k,pred})\text{\hspace{1em}(9)}$$

$${\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k){\mathbf{P}}_{k,pred}\text{\hspace{1em}(10)}$$

修改部分代码如下，

void EkfPredict(State& state, const double &time, const double &velocity, const double &yaw_rate) {
    // printf("time %lf, velocity %lf, yaw_rate %lf \n", time, velocity, yaw_rate);
    // YOUR_CODE_HERE
    // todo: implement the EkfPredict function

    std::cout << "================start predict...================" << std::endl;
    State pred;
    pred.time = time;
    double delta_t = 0.01;
    //均值部分
    pred.x = state.x + velocity * std::cos(state.yaw) * delta_t;
    pred.y = state.y + velocity * std::sin(state.yaw) * delta_t; 
    pred.yaw = state.yaw + yaw_rate * delta_t;
    if (pred.yaw > M_PI) { //把pred.yaw限制在[-pi,pi]
        pred.yaw -= 2 * M_PI;
    } else if (pred.yaw < -M_PI) {
        pred.yaw += 2 * M_PI;
    }
    std::cout << "x = " << pred.x << ", y = " << pred.y << ", yaw = " << pred.yaw << std::endl;

    //方差部分
    Eigen::Matrix3d A_k;
    A_k << 1.0, 0.0, -velocity * std::sin(state.yaw) * delta_t,
           0.0, 1.0, velocity * std::cos(state.yaw) * delta_t,
           0.0, 0.0, 1.0;
    Eigen::Matrix<double, 3, 2> B_k;
    B_k << std::cos(state.yaw) * delta_t, 0.0, 
           std::sin(state.yaw) * delta_t, 0.0,
           0.0, delta_t; 
    pred.P = A_k * state.P * A_k.transpose() + B_k * Qn * B_k.transpose();
    std::cout << "A_k: \n" << A_k << std::endl;
    std::cout << "B_k: \n" << B_k << std::endl;
    std::cout << "Qn: \n" << Qn << std::endl;
    std::cout << "P: \n" << pred.P << std::endl;

    state.time = pred.time;
    state.x = pred.x;
    state.y = pred.y;
    state.yaw = pred.yaw;
    state.P = pred.P;
    std::cout << "================end predict...================" << std::endl;
    return;

    // printf("after predict x: %lf, y: %lf, yaw: %lf \n", state.x, state.y, state.yaw);
}

void EkfUpdate(State& state,  const double &m_x, const double &m_y, const double &m_yaw) {
    // printf("time :%lf \n", state.time);
    // printf("before update x: %lf, y: %lf, yaw: %lf \n", state.x, state.y, state.yaw);
    // printf("measure x: %lf, y: %lf, yaw: %lf \n", m_x, m_y, m_yaw);
    // YOUR_CODE_HERE
    // todo: implement the EkfUpdate function

    std::cout << "================start update ...================" << std::endl;

    State update;
    
    //计算卡尔曼增益 
    Eigen::Matrix3d K_k = state.P * (state.P + Rn).inverse(); 
    std::cout << "K_k:\n" << K_k << std::endl;
    
    //计算均值部分
    Eigen::Vector3d z_k(m_x, m_y, m_yaw);
    Eigen::Vector3d x_k_pred(state.x, state.y, state.yaw);
    Eigen::Vector3d innovation = z_k - x_k_pred;
    if (innovation[2] > M_PI) { //将yaw之间的差异转换到[-pi,pi]之间 
        innovation[2] -= 2.0 * M_PI;
    } else if (innovation[2] < -M_PI) {
        innovation[2] += 2.0 * M_PI;
    }
    Eigen::Vector3d x_k = x_k_pred + K_k * innovation;
    update.x = x_k[0];
    update.y = x_k[1];
    update.yaw = x_k[2];
    // if (update.yaw > M_PI) {
    //     update.yaw -= 2 * M_PI;
    // } else if (update.yaw < -M_PI) {
    //     update.yaw += 2 * M_PI;
    // }
    std::cout << "x = " << update.x << ", y = " << update.y << ", yaw = " << update.yaw << std::endl;

    //计算方差部分
    update.P = (Eigen::Matrix3d::Identity() - K_k) * state.P;
    std::cout << "P: \n" << update.P << std::endl;

    state.x = update.x;
    state.y = update.y;
    state.yaw = update.yaw;
    state.P = update.P;
    std::cout << "================end update...================" << std::endl;
    return;
    // printf("after update x: %lf, y: %lf, yaw: %lf \n", state.x, state.y, state.yaw);
}

打开4个终端，依次分别输入，

catkin_make

roscore

bash devel/setup.bash
rosrun vehicle_state_estimation vehicle_state_estimation

bash devel/setup.bash
rosrun rviz rviz -d src/state_estimation.rviz

rviz展示，