零速检测，粗略估计陀螺零偏以及俯仰角、横滚角

零速检测，粗略估计陀螺零偏以及俯仰角、横滚角

首先定义一个imu_buff存储imu观测值，大小可以自己视情况而定，imu采样200hz，即5ms输出一个数据，通过40-100ms即8至20个IMU数据，一般size=20即可。

    // 缓存数据用于零速检测
    // Buffer for zero-velocity detection
    vector<IMU> imu_buff;
    for (const auto &ins : ins_window_) {
        auto &imu = ins.first;
        if ((imu.time > last_gnss_.time) && (imu.time < gnss_.time)) {
            imu_buff.push_back(imu);
        }
    }
    if (imu_buff.size() < 20) {
        return false;
    }

存储完，开始检测零速，计算该时间段内(size=20)的速率(标准差×数据率)，判断是否小于阈值，小于阈值则判断为零速。

    // 零速阈值, rad/s, m/s^2
    static constexpr double ZERO_VELOCITY_GYR_THRESHOLD = 0.002;
    static constexpr double ZERO_VELOCITY_ACC_THRESHOLD = 0.1;

bool MISC::detectZeroVelocity(const vector<IMU> &imu_buffer, double imudatarate, vector<double> &average) {

    auto size          = static_cast<double>(imu_buffer.size());
    double size_invert = 1.0 / size;

    double sum[6];
    double std[6];

    average.resize(6);
    average[0] = average[1] = average[2] = average[3] = average[4] = average[5] = 0;
    for (const auto &imu : imu_buffer) {
        average[0] += imu.dtheta.x();
        average[1] += imu.dtheta.y();
        average[2] += imu.dtheta.z();
        average[3] += imu.dvel.x();
        average[4] += imu.dvel.y();
        average[5] += imu.dvel.z();
    }

    average[0] *= size_invert;
    average[1] *= size_invert;
    average[2] *= size_invert;
    average[3] *= size_invert;
    average[4] *= size_invert;
    average[5] *= size_invert;

    sum[0] = sum[1] = sum[2] = sum[3] = sum[4] = sum[5] = 0;
    for (const auto &imu : imu_buffer) {
        sum[0] += (imu.dtheta.x() - average[0]) * (imu.dtheta.x() - average[0]);
        sum[1] += (imu.dtheta.y() - average[1]) * (imu.dtheta.y() - average[1]);
        sum[2] += (imu.dtheta.z() - average[2]) * (imu.dtheta.z() - average[2]);
        sum[3] += (imu.dvel.x() - average[3]) * (imu.dvel.x() - average[3]);
        sum[4] += (imu.dvel.y() - average[4]) * (imu.dvel.y() - average[4]);
        sum[5] += (imu.dvel.z() - average[5]) * (imu.dvel.z() - average[5]);
    }

    // 速率形式
    std[0] = sqrt(sum[0] * size_invert) * imudatarate;
    std[1] = sqrt(sum[1] * size_invert) * imudatarate;
    std[2] = sqrt(sum[2] * size_invert) * imudatarate;
    std[3] = sqrt(sum[3] * size_invert) * imudatarate;
    std[4] = sqrt(sum[4] * size_invert) * imudatarate;
    std[5] = sqrt(sum[5] * size_invert) * imudatarate;

    if ((std[0] < ZERO_VELOCITY_GYR_THRESHOLD) && (std[1] < ZERO_VELOCITY_GYR_THRESHOLD) &&
        (std[2] < ZERO_VELOCITY_GYR_THRESHOLD) && (std[3] < ZERO_VELOCITY_ACC_THRESHOLD) &&
        (std[4] < ZERO_VELOCITY_ACC_THRESHOLD) && (std[5] < ZERO_VELOCITY_ACC_THRESHOLD)) {

        return true;
    }

    return false;
}

判断出零速之后，粗略估计陀螺零偏以及俯仰角和横滚角

if (is_zero_velocity) {
        // 陀螺零偏
        bg = Vector3d(average[0], average[1], average[2]);
        bg *= imudatarate_;

        // 重力调平获取横滚俯仰角
        Vector3d fb(average[3], average[4], average[5]);
        fb *= imudatarate_;

        initatt[0] = -asin(fb[1] / integration_parameters_->gravity);
        initatt[1] = asin(fb[0] / integration_parameters_->gravity);

        LOGI << "Zero velocity get gyroscope bias " << bg.transpose() * 3600 * R2D << ", roll " << initatt[0] * R2D
             << ", pitch " << initatt[1] * R2D;
        is_has_zero_velocity = true;
    }