kf-gins

// 速度误差

    temp.setZero();

    temp(0, 0) = -2 * pvapre_.vel[1] * WGS84_WIE * cos(pvapre_.pos[0]) / rmh -

                 pow(pvapre_.vel[1], 2) / rmh / rnh / pow(cos(pvapre_.pos[0]), 2);

    temp(0, 2) = pvapre_.vel[0] * pvapre_.vel[2] / rmh / rmh

                - pow(pvapre_.vel[1], 2)  * tan(pvapre_.pos[0]) / rnh / rnh;

    temp(1, 0) = 2 * WGS84_WIE * (pvapre_.vel[0] * cos(pvapre_.pos[0])

                -pvapre_.vel[2] * sin(pvapre_.pos[0])) / rmh +

                 pvapre_.vel[0] * pvapre_.vel[1] / rmh / rnh

                /pow(cos(pvapre_.pos[0]), 2);

    temp(1, 2) = (pvapre_.vel[1] * pvapre_.vel[2]

               +pvapre_.vel[0] * pvapre_.vel[1] * tan(pvapre_.pos[0])) / rnh / rnh;

    temp(2, 0) = 2 * WGS84_WIE * pvapre_.vel[1] * sin(pvapre_.pos[0]) / rmh;

    temp(2, 2) = -pow(pvapre_.vel[1], 2) / rnh / rnh

                - pow(pvapre_.vel[0], 2) / rmh / rmh +

                 2 * gravity / (sqrt(rmrn[0] * rmrn[1]) + pvapre_.pos[2]);

    F.block(V_ID, P_ID, 3, 3) = temp;