SAD notes

2023-10-26 09:53:32 终极管理员知识笔记 38阅读

ESKF 总结 prediction

更新误差先验

F F F通过3.42来算

得到

这里有点绕的一点是: 误差状态的 F F F牵涉到名义状态, 而名义状态又需要在时间上推进更新

其中, F中的名义状态的推进通过公式3.41得到,

(名义状态不考虑误差, 这一点从3.41d, 3.41e可以看出, 误差状态只考虑误差, 真实状态为名义误差)

代码的实现

template <typename S>bool ESKF<S>::Predict(const IMU& imu) {    assert(imu.timestamp_ > current_time_);    double dt  imu.timestamp_ - current_time_;    if (dt > (5 * options_.imu_dt_) || dt < 0) {        // 时间间隔不对可能是第一个IMU数据没有历史信息        LOG(INFO) << skip this imu because dt_   << dt;        current_time_  imu.timestamp_;        return false;    }    // nominal state 递推    VecT new_p  p_  v_ * dt  0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt  0.5 * g_ * dt * dt;    VecT new_v  v_  R_ * (imu.acce_ - ba_) * dt  g_ * dt;    SO3 new_R  R_ * SO3::exp((imu.gyro_ - bg_) * dt);    R_  new_R;    v_  new_v;    p_  new_p;    // 其余状态维度不变    // error state 递推    // 计算运动过程雅可比矩阵 F见(3.47)    // F实际上是稀疏矩阵也可以不用矩阵形式进行相乘而是写成散装形式这里为了教学方便使用矩阵形式    Mat18T F  Mat18T::Identity();                                                 // 主对角线    F.template block<3, 3>(0, 3)  Mat3T::Identity() * dt;                         // p 对 v    F.template block<3, 3>(3, 6)  -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;  // v对theta    F.template block<3, 3>(3, 12)  -R_.matrix() * dt;                             // v 对 ba    F.template block<3, 3>(3, 15)  Mat3T::Identity() * dt;                        // v 对 g    F.template block<3, 3>(6, 6)  SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();     // theta 对 theta    F.template block<3, 3>(6, 9)  -Mat3T::Identity() * dt;                        // theta 对 bg    // mean and cov prediction    dx_  F * dx_;  // 这行其实没必要算dx_在重置之后应该为零因此这步可以跳过但F需要参与Cov部分计算所以保留    cov_  F * cov_.eval() * F.transpose()  Q_;     current_time_  imu.timestamp_;    return true;}

Q Q Q的算法

注意, 在离散情况下
η v η a Δ t η θ η g Δ t η g η b g Δ t η a η b a Δ t \begin{aligned} &\eta_v \eta_a \Delta t\\ &\eta_ \theta \eta_g \Delta t \\ &\eta_g \eta_{bg} \Delta t\\ &\eta_ a \eta_{ba} \Delta t \\ \end{aligned} ηvηaΔtηθηgΔtηgηbgΔtηaηbaΔt
可以根据3.40将3.42进行一阶泰勒展开

代码中的实现

        double ev  options.acce_var_;        double et  options.gyro_var_;        double eg  options.bias_gyro_var_;        double ea  options.bias_acce_var_;        double ev2  ev;  // * ev;   // tj : 为什么没平方? Q里面应该是方差        double et2  et;  // * et;        double eg2  eg;  // * eg;        double ea2  ea;  // * ea;        // 设置过程噪声        Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;

其中options的定义

    struct Options {        Options()  default;        /// IMU 测量与零偏参数        double imu_dt_  0.01;  // IMU测量间隔        // NOTE IMU噪声项都为离散时间不需要再乘dt可以由初始化器指定IMU噪声        double gyro_var_  1e-5;       // 陀螺测量标准差        double acce_var_  1e-2;       // 加计测量标准差        double bias_gyro_var_  1e-6;  // 陀螺零偏游走标准差        double bias_acce_var_  1e-4;  // 加计零偏游走标准差

correction

更新误差后验

H H H是观测值对误差状态变量的jacob

先看GNSS, 它观测值有位移和旋转, 根据3.66和3.70可得 H H H,

GNSS对旋转的观测是总体的旋转, 然而名义上的 R R R是知道的, 所以我们可以将观测值变以成对于旋转误差状态的直接观测, 这样一来, 它对自己求导就为 I I I

同理, GNSS对位移的观测是总体的位移, 然后名义上的 p p p是知道, 所以我们将对总体位移的观测转到对于误差位移的直接观测

    Eigen::Matrix<S, 6, 18> H  Eigen::Matrix<S, 6, 18>::Zero();    H.template block<3, 3>(0, 0)  Mat3T::Identity();  // P部分 (3.70)    H.template block<3, 3>(3, 6)  Mat3T::Identity();  // R部分3.66)

然后求 V V V

 // 卡尔曼增益和更新过程    Vec6d noise_vec;    noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;    Mat6d V  noise_vec.asDiagonal();

其中trans_noise, ang_noise在option中定义

        /// RTK 观测参数        double gnss_pos_noise_  0.1;                   // GNSS位置噪声        double gnss_height_noise_  0.1;                // GNSS高度噪声        double gnss_ang_noise_  1.0 * math::kDEG2RAD;  // GNSS旋转噪声

然后更新后验3.51b, 3.51d

    // 更新x和cov    Vec6d innov  Vec6d::Zero();    innov.template head<3>()  (pose.translation() - p_);          // 平移部分    innov.template tail<3>()  (R_.inverse() * pose.so3()).log();  // 旋转部分(3.67)    dx_  K * innov;    cov_  (Mat18T::Identity() - K * H) * cov_;

有了误差状态, 就可以更新名义状态3.51c

   /// 更新名义状态变量重置error state    void UpdateAndReset() {        p_  dx_.template block<3, 1>(0, 0);        v_  dx_.template block<3, 1>(3, 0);        R_  R_ * SO3::exp(dx_.template block<3, 1>(6, 0));        if (options_.update_bias_gyro_) {            bg_  dx_.template block<3, 1>(9, 0);        }        if (options_.update_bias_acce_) {            ba_  dx_.template block<3, 1>(12, 0);        }        g_  dx_.template block<3, 1>(15, 0);        ProjectCov();        dx_.setZero();    }

注意这里有一步需要投影, 参考3.63

/// 对P阵进行投影参考式(3.63)void ProjectCov() {    Mat18T J  Mat18T::Identity();    J.template block<3, 3>(6, 6)  Mat3T::Identity() - 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));    cov_  J * cov_ * J.transpose();}

再看odom, 它观测值只有速度

首先求H, 注意这里观测值为本地速度, 但是名义变量 R R R这时是知道的, 所以可以把观测值从本地速度转化到世界速度, 然后世界速度对世界速度求导就为 I I I

// odom 修正以及雅可比// 使用三维的轮速观测H为3x18大部分为零Eigen::Matrix<S, 3, 18> H  Eigen::Matrix<S, 3, 18>::Zero();H.template block<3, 3>(0, 3)  Mat3T::Identity();

然后算V

// 设置里程计噪声double o2  options_.odom_var_ * options_.odom_var_;odom_noise_.diagonal() << o2, o2, o2;

option里面的定义

/// 里程计参数double odom_var_  0.5;double odom_span_  0.1;        // 里程计测量间隔double wheel_radius_  0.155;   // 轮子半径double circle_pulse_  1024.0;  // 编码器每圈脉冲数

然后更新后验

本地速度观测值的算法参见 3.76

// velocity obsdouble velo_l  options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;double velo_r     options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;double average_vel  0.5 * (velo_l  velo_r);VecT vel_odom(average_vel, 0.0, 0.0);VecT vel_world  R_ * vel_odom;dx_  K * (vel_world - v_);// update covcov_  (Mat18T::Identity() - K * H) * cov_;UpdateAndReset();

标签：