gi_engine.cpp

GIEngine::GIEngine(GINSOptions &options) {

    // 初始化系统噪声阵

   auto imunoise = options_.imunoise;

    Qc_.block(ARW_ID, ARW_ID, 3, 3) = imunoise.gyr_arw.cwiseProduct(imunoise.gyr_arw).asDiagonal();

    Qc_.block(VRW_ID, VRW_ID, 3, 3) = imunoise.acc_vrw.cwiseProduct(imunoise.acc_vrw).asDiagonal();

    Qc_.block(BGSTD_ID, BGSTD_ID, 3, 3) =

        2 / imunoise.corr_time * imunoise.gyrbias_std.cwiseProduct(imunoise.gyrbias_std).asDiagonal();

    Qc_.block(BASTD_ID, BASTD_ID, 3, 3) =

        2 / imunoise.corr_time * imunoise.accbias_std.cwiseProduct(imunoise.accbias_std).asDiagonal();

    Qc_.block(SGSTD_ID, SGSTD_ID, 3, 3) =

        2 / imunoise.corr_time * imunoise.gyrscale_std.cwiseProduct(imunoise.gyrscale_std).asDiagonal();

    Qc_.block(SASTD_ID, SASTD_ID, 3, 3) =

        2 / imunoise.corr_time * imunoise.accscale_std.cwiseProduct(imunoise.accscale_std).asDiagonal();