KF-GINS

KF-GINS学习笔记

KF-GINS一套基于扩展卡尔曼滤波的GNSS/INS组合导航算法代码,通过松耦合的方式将IMU和GNSS信息进行融合提高定位精度，并且可以在GNSS信号确实的情况下在短时间内提供可靠的定位。

IMU误差补偿

捷联惯性导航算法之前进行误差补偿，补偿当前估计的零偏和比例因子

误差模型：
$$\begin{array}{rl}\delta {\mathbf{\omega }}_{ib}^b& ={\mathbf{b}}_g+diag\left({\mathbf{s}}_g\right){\mathbf{\omega }}_{ib}^b+{\mathbf{w}}_g\\ \delta {\mathbf{f}}_{ib}^b& ={\mathbf{b}}_a+diag\left({\mathbf{s}}_a\right){\mathbf{f}}_{ib}^b+{\mathbf{w}}_a\end{array}$$
误差补偿：
$$\begin{array}{rl}& diag{\left(\mathbf{I}+{\overline{\mathbf{s}}}_g\right)}^{-1}({\tilde{\mathbf{\omega }}}_{ib}^b-{\bar{\mathbf{b}}}_g)={\hat{\mathbf{\omega }}}_{ib}^b\\ & diag{\left(\mathbf{I}+{\overline{\mathbf{s}}}_a\right)}^{-1}({\tilde{\mathbf{f}}}_{ib}^b-{\bar{\mathbf{b}}}_a)={\hat{\mathbf{f}}}_{ib}^b\end{array}$$

INS机械编排

通过IMU发布的信息和过去导航状态推算更新当前导航状态

已知:$\Delta {\mathbf{v}}_k,\Delta {\mathbf{\theta }}_k\Delta {\mathbf{v}}_{k-1},\Delta {\mathbf{\theta }}_{k-1},{\mathbf{v}}_{eb,k-1}^n,h_{k-1},{\phi }_{k-1},{\lambda }_{k-1},{\mathbf{q}}_{b_{k-1}}^{n_{k-1}}$

求解:${\mathbf{v}}_{eb,k}^n,h_k,{\phi }_k,{\lambda }_k,{\mathbf{q}}_{b_k}^{n_k}$

速度更新

$n$系下速度微分方程(站在$n$系角度观测到数值随时间的变换率)：
$${\frac{d\mathbf{v}}{dt}|}_n=\mathbf{f}-(2{\mathbf{\omega }}_{ie}+{\mathbf{\omega }}_{en})\times \mathbf{v}+{\mathbf{g}}_p$$
将向量投影到$n$系(即在n系的坐标值)：
$$\begin{array}{rl}{\frac{d\mathbf{v}}{dt}|}_n^n& ={\mathbf{f}}^n-(2{\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n)\times {\mathbf{v}}^n+{\mathbf{g}}_p^n\\ & ={\mathbf{C}}_b^n{\mathbf{f}}^b-(2{\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n)\times {\mathbf{v}}^n+{\mathbf{g}}_p^n\end{array}$$
对其进行离散积分：
$$\begin{array}{c}\Delta {\mathbf{v}}_{f,k}^n\equiv {\int }_{t_{k-1}}^{t_k}{\mathbf{C}}_b^n{\mathbf{f}}^{b}dt\\ \Delta {\mathbf{v}}_{g/cor,k}^n\equiv {\int }_{t_{k-1}}^{t_k}\left[{\mathbf{g}}_p^n-(2{\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n)\times {\mathbf{v}}^n\right]dt\end{array}$$
速度更新：
$${\mathbf{v}}_k^n={\mathbf{v}}_{k-1}^n+\Delta {\mathbf{v}}_{f,k}^n+\Delta {\mathbf{v}}_{g/cor,k}^n$$
第二项为比力积分项，第三项为重力/科氏力积分项

对积分进行近似化处理（等效旋转矢量，双子样假设）：
$$\begin{array}{c}\Delta {\mathbf{v}}_{g/cor,k}^n={\left[{\mathbf{g}}_p^n-(2{\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n)\times {\mathbf{v}}^n\right]}_{t_{k-1/2}}\Delta t\\ \Delta {\mathbf{v}}_{f,k}^n=\left[\mathbf{I}-\frac{1}{2}({\mathbf{\zeta }}_{n(k-1),n(k)}\times )\right]{\mathbf{C}}_{b(k-1)}^{n(k-1)}\Delta {\mathbf{v}}_{f,k}^{b(k-1)}\\ {\zeta }_{n(k-1),n(k)}={\left({\mathbf{\omega }}_{ie}+{\mathbf{\omega }}_{en}\right)}_{t_{k-1/2}}\Delta t\\ \Delta {\mathbf{v}}_{f,k}^{b(k-1)}=\Delta {\mathbf{v}}_k+\frac{1}{2}\Delta {\mathbf{\theta }}_k\times \Delta {\mathbf{v}}_k+\frac{1}{12}\left(\Delta {\mathbf{\theta }}_{k-1}\times \Delta {\mathbf{v}}_k+\Delta {\mathbf{v}}_{k-1}\times \Delta {\mathbf{\theta }}_k\right)\end{array}$$

void INSMech::velUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const IMU &imucur) {
    
    

    Eigen::Vector3d d_vfb, d_vfn, d_vgn, gl, midvel, midpos;
    Eigen::Vector3d temp1, temp2, temp3;
    Eigen::Matrix3d cnn, I33 = Eigen::Matrix3d::Identity();
    Eigen::Quaterniond qne, qee, qnn, qbb, q1, q2;

    // 计算地理参数，子午圈半径和卯酉圈半径，地球自转角速度投影到n系, n系相对于e系转动角速度投影到n系，重力值
    // calculate geographic parameters, Meridian and Mao unitary radii,
    // earth rotational angular velocity projected to n-frame,
    // rotational angular velocity of n-frame to e-frame projected to n-frame, and gravity
    Eigen::Vector2d rmrn = Earth::meridianPrimeVerticalRadius(pvapre.pos(0));
    Eigen::Vector3d wie_n, wen_n;
    wie_n << WGS84_WIE * cos(pvapre.pos[0]), 0, -WGS84_WIE * sin(pvapre.pos[0]);
    wen_n << pvapre.vel[1] / (rmrn[1] + pvapre.pos[2]), -pvapre.vel[0] / (rmrn[0] + pvapre.pos[2]),
        -pvapre.vel[1] * tan(pvapre.pos[0]) / (rmrn[1] + pvapre.pos[2]);
    double gravity = Earth::gravity(pvapre.pos);

    // 旋转效应和双子样划桨效应
    // rotational and sculling motion
    temp1 = imucur.dtheta.cross(imucur.dvel) / 2;
    temp2 = imupre.dtheta.cross(imucur.dvel) / 12;
    temp3 = imupre.dvel.cross(imucur.dtheta) / 12;

    // b系比力积分项
    // velocity increment due to the specific force
    d_vfb = imucur.dvel + temp1 + temp2 + temp3;

    // 比力积分项投影到n系
    // velocity increment dut to the specfic force projected to the n-frame
    temp1 = (wie_n + wen_n) * imucur.dt / 2;
    cnn   = I33 - Rotation::skewSymmetric(temp1);
    d_vfn = cnn * pvapre.att.cbn * d_vfb;

    // 计算重力/哥式积分项
    // velocity increment due to the gravity and Coriolis force
    gl << 0, 0, gravity;
    d_vgn = (gl - (2 * wie_n + wen_n).cross(pvapre.vel)) * imucur.dt;

    // 得到中间时刻速度
    // velocity at k-1/2
    midvel = pvapre.vel + (d_vfn + d_vgn) / 2;

    // 外推得到中间时刻位置
    // position extrapolation to k-1/2
    qnn = Rotation::rotvec2quaternion(temp1);
    temp2 << 0, 0, -WGS84_WIE * imucur.dt / 2;
    qee = Rotation::rotvec2quaternion(temp2);
    qne = Earth::qne(pvapre.pos);
    qne = qee * qne * qnn;
    midpos[2] = pvapre.pos[2] - midvel[2] * imucur.dt / 2;
    midpos    = Earth::blh(qne, midpos[2]);

    // 重新计算中间时刻的rmrn, wie_e, wen_n
    // recompute rmrn, wie_n, and wen_n at k-1/2
    rmrn = Earth::meridianPrimeVerticalRadius(midpos[0]);
    wie_n << WGS84_WIE * cos(midpos[0]), 0, -WGS84_WIE * sin(midpos[0]);
    wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
        -midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);

    // 重新计算n系下平均比力积分项
    // recompute d_vfn
    temp3 = (wie_n + wen_n) * imucur.dt / 2;
    cnn   = I33 - Rotation::skewSymmetric(temp3);
    d_vfn = cnn * pvapre.att.cbn * d_vfb;

    // 重新计算重力、哥式积分项
    // recompute d_vgn
    gl << 0, 0, Earth::gravity(midpos);
    d_vgn = (gl - (2 * wie_n + wen_n).cross(midvel)) * imucur.dt;

    // 速度更新完成
    // velocity update finish
    pvacur.vel = pvapre.vel + d_vfn + d_vgn;
}

位置更新

大地坐标系下位置微分方程：
$$\begin{array}{c}\dot{\phi }=\frac{v_N}{R_M+h}\\ \dot{\lambda }=\frac{v_E}{(R_N+h)\cos \phi }\\ \dot{h}=-v_D\end{array}$$
对积分进行近似化处理：
$$\begin{gathered} h_k=h_{k-1}-\frac{1}{2}(v_{D,k-1}+v_{D,k})(t_k-t_{k-1}) \\ {\phi }_k={\phi }_{k-1}+\frac{v_{N,k}+v_{N,k-1}}{2(R_{M,k-1}+\bar{h})}(t_k-t_{k-1}) \\ {\lambda }_k={\lambda }_{k-1}+\frac{v_{E,k}+v_{E,k-1}}{2(R_{N,k-1/2}+\bar{h})\cos \bar{\phi }}(t_k-t_{k-1}) \end{gathered}$$
其中，$R_{N,k-1/2}$ 表示用中间时刻的纬度计算 $R_N,\bar{h}=\frac{1}{2}(h_k+h_{k-1}),\bar{\phi }=\frac{1}{2}({\phi }_k+{\phi }_{k-1})$

void INSMech::posUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const IMU &imucur) {
    
    

    Eigen::Vector3d temp1, temp2, midvel, midpos;
    Eigen::Quaterniond qne, qee, qnn;

    // 重新计算中间时刻的速度和位置
    // recompute velocity and position at k-1/2
    midvel = (pvacur.vel + pvapre.vel) / 2;
    midpos = pvapre.pos + Earth::DRi(pvapre.pos) * midvel * imucur.dt / 2;

    // 重新计算中间时刻地理参数
    // recompute rmrn, wie_n, wen_n at k-1/2
    Eigen::Vector2d rmrn;
    Eigen::Vector3d wie_n, wen_n;
    rmrn = Earth::meridianPrimeVerticalRadius(midpos[0]);
    wie_n << WGS84_WIE * cos(midpos[0]), 0, -WGS84_WIE * sin(midpos[0]);
    wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
        -midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);

    // 重新计算 k时刻到k-1时刻 n系旋转矢量
    // recompute n-frame rotation vector (n(k) with respect to n(k-1)-frame)
    temp1 = (wie_n + wen_n) * imucur.dt;
    qnn   = Rotation::rotvec2quaternion(temp1);
    // e系转动等效旋转矢量 (k-1时刻k时刻，所以取负号)
    // e-frame rotation vector (e(k-1) with respect to e(k)-frame)
    temp2 << 0, 0, -WGS84_WIE * imucur.dt;
    qee = Rotation::rotvec2quaternion(temp2);

    // 位置更新完成
    // position update finish
    qne           = Earth::qne(pvapre.pos);
    qne           = qee * qne * qnn;
    pvacur.pos[2] = pvapre.pos[2] - midvel[2] * imucur.dt;
    pvacur.pos    = Earth::blh(qne, pvacur.pos[2]);
}

姿态更新

姿态的更新计算可用四元数的如下连乘运算来实现：
$${\mathbf{q}}_{b(k)}^{n(k)}={\mathbf{q}}_{n(k-1)}^{n(k)}\circ {\mathbf{q}}_{b(k-1)}^{n(k-1)}\circ {\mathbf{q}}_{b(k)}^{b(k-1)}$$
$q_{b(k-1)}^{n(k-1)}$为上一时刻姿态已知，计算$q_{n(k-1)}^{n(k)}$和$q_{b(k)}^{b(k-1)}$
$${\mathbf{q}}_{b(k)}^{b(k-1)}=\left[\begin{array}{c}\cos \Vert 0.5{\mathbf{\varphi }}_k\Vert \\ \\ \frac{\sin \Vert 0.5{\mathbf{\varphi }}_k\Vert }{\Vert 0.5{\mathbf{\varphi }}_k\Vert }0.5{\mathbf{\varphi }}_k\end{array}\right]$$
等效旋转矢量近似化处理对应（双子样假设）：
$${\varphi }_k=\Delta {\mathbf{\theta }}_k+\frac{1}{12}\Delta {\mathbf{\theta }}_{k-1}\times \Delta {\mathbf{\theta }}_k$$

$${\mathbf{q}}_{n(k-1)}^{n(k)}=\left[\begin{array}{c}\cos \Vert 0.5{\mathbf{\zeta }}_k\Vert \\ \\ -\frac{\sin \Vert 0.5{\mathbf{\zeta }}_k\Vert }{\Vert 0.5{\mathbf{\zeta }}_k\Vert }0.5{\mathbf{\zeta }}_k\end{array}\right]$$

等效旋转矢量对应：
$${\zeta }_k={\int }_{t_{k-1}}^{t_k}\left[{\omega }_{ie}^n(t)+{\omega }_{en}^n(t)\right]dt\approx \left[{\omega }_{ie}^n(t_{k-1})+{\omega }_{en}^n(t_{k-1})\right]\Delta t$$
其中，
$$\begin{array}{c}{\omega }_{ie}^n={\left[\begin{array}{ccc}{\omega }_e\cos \phi & 0& -{\omega }_e\sin \phi \end{array}\right]}^{\mathrm{T}}\\ {\omega }_{en}^n=\left[\begin{array}{c}{v}_E/(R_N+h)\\ -v_N/(R_M+h)\\ -v_E\tan \phi /(R_N+h)\end{array}\right]\\ R_M=\frac{a(1-e^2)}{\sqrt{(1-e^2{\sin }^2\phi {)}^3}}\\ R_N=\frac{a}{\sqrt{1-e^2{\sin }^2\phi }}\end{array}$$
式中 $\phi$ 为纬度（单位弧度），$v_E$，$v_N$ 分别为东向和北向速度。$R_M$ 和 $R_N$ 分别为子午圈半径和卯酉圈 半径。$a$ 为地球椭球长半轴，$e $为地球椭球模型第一偏心率，对于CGCS2000椭球，$a = 6378137.0m$， $e=0.08181919104$

void INSMech::attUpdate(const PVA &pvapre, PVA &pvacur, const IMU &imupre, const IMU &imucur) {
    
    

    Eigen::Quaterniond qne_pre, qne_cur, qne_mid, qnn, qbb;
    Eigen::Vector3d temp1, midpos, midvel;

    // 重新计算中间时刻的速度和位置
    // recompute velocity and position at k-1/2
    midvel = (pvapre.vel + pvacur.vel) / 2;
    qne_pre   = Earth::qne(pvapre.pos);
    qne_cur   = Earth::qne(pvacur.pos);
    temp1     = Rotation::quaternion2vector(qne_cur.inverse() * qne_pre);
    qne_mid   = qne_pre * Rotation::rotvec2quaternion(temp1 / 2).inverse();
    midpos[2] = (pvacur.pos[2] + pvapre.pos[2]) / 2;
    midpos    = Earth::blh(qne_mid, midpos[2]);

    // 重新计算中间时刻地理参数
    // recompute rmrn, wie_n, wen_n at k-1/2
    Eigen::Vector2d rmrn;
    Eigen::Vector3d wie_n, wen_n;
    rmrn = Earth::meridianPrimeVerticalRadius(midpos[0]);
    wie_n << WGS84_WIE * cos(midpos[0]), 0, -WGS84_WIE * sin(midpos[0]);
    wen_n << midvel[1] / (rmrn[1] + midpos[2]), -midvel[0] / (rmrn[0] + midpos[2]),
        -midvel[1] * tan(midpos[0]) / (rmrn[1] + midpos[2]);

    // 计算n系的旋转四元数 k-1时刻到k时刻变换
    // n-frame rotation vector (n(k-1) with respect to n(k)-frame)
    temp1 = -(wie_n + wen_n) * imucur.dt;
    qnn   = Rotation::rotvec2quaternion(temp1);

    // 计算b系旋转四元数 补偿二阶圆锥误差
    // b-frame rotation vector (b(k) with respect to b(k-1)-frame)
    // compensate the second-order coning correction term.
    temp1 = imucur.dtheta + imupre.dtheta.cross(imucur.dtheta) / 12;
    qbb   = Rotation::rotvec2quaternion(temp1);

    // 姿态更新完成
    // attitude update finish
    pvacur.att.qbn   = qnn * pvapre.att.qbn * qbb;
    pvacur.att.cbn   = Rotation::quaternion2matrix(pvacur.att.qbn);
    pvacur.att.euler = Rotation::matrix2euler(pvacur.att.cbn);
}

误差微分方程

姿态误差微分方程（忽略二阶小量）：
$$\dot{\varphi }=-{\omega }_{in}^n\times \varphi +\delta {\mathbf{\omega }}_{in}^n-\delta {\mathbf{\omega }}_{ib}^n$$
展开：
$$\begin{array}{rl}& {\dot{\varphi }}_N=-\frac{{\omega }_e\sin \phi }{R_M+h}\delta r_N+\frac{v_E}{(R_N+h{)}^2}\delta r_D+\frac{1}{R_N+h}\delta v_E-\left({\omega }_e\sin \phi +\frac{v_E\tan \phi }{R_N+h}\right){\varphi }_E\\ & +\frac{v_N}{R_M+h}{\varphi }_D-\delta {\omega }_{ib,N}^n\\ & {\dot{\varphi }}_E=\begin{array}{r}-\frac{v_N}{(R_M+h{)}^2}\delta r_D-\frac{1}{R_M+h}\delta v_N+\left({\omega }_e\sin \phi +\frac{v_E\tan \phi }{R_N+h}\right){\varphi }_N\end{array}\\ & +\left({\omega }_e\cos \phi +\frac{v_E}{R_N+h}\right){\varphi }_D-\delta {\omega }_{ib,E}^n\\ & {\dot{\varphi }}_D=\begin{array}{rl}& -\left[\frac{{\omega }_e\cos \phi }{R_M+h}+\frac{v_E{\sec }^2\phi }{(R_M+h)(R_N+h)}\right]\delta r_N-\frac{v_E\tan \phi }{(R_N+h{)}^2}\delta r_D-\frac{\tan \phi }{R_N+h}\delta v_E\end{array}\\ & -\frac{v_N}{R_M+h}{\varphi }_N-\left({\omega }_e\cos \phi +\frac{v_E}{R_N+h}\right){\varphi }_E-\delta {\omega }_{ib,D}^n\end{array}$$
速度误差微分方程（忽略二阶小量）：
$$\delta {\dot{\mathbf{v}}}^n={\mathbf{C}}_b^n\delta {\mathbf{f}}^b+{\mathbf{f}}^n\times \mathbf{\varphi }-(2{\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n)\times \delta {\mathbf{v}}^n+{\mathbf{v}}^n\times (2\delta {\mathbf{\omega }}_{ie}^n+\delta {\mathbf{\omega }}_{en}^n)+\delta {\mathbf{g}}_p^n$$
展开：
$$\begin{array}{rlrl}& \delta {\dot{v}}_N& & =-\left[\frac{2v_E{\omega }_e\cos \phi }{R_M+h}+\frac{v_E^2{\sec }^2\phi }{(R_M+h)(R_N+h)}\right]\delta r_N+\left[\frac{v_N{v}_D}{(R_M+h{)}^2}-\frac{v_E^2\tan \phi }{(R_N+h{)}^2}\right]\delta r_D\\ & & & \begin{array}{r}+\frac{v_D}{R_M+h}\delta v_N-2\left({\omega }_e\sin \phi +\frac{v_E\tan \phi }{R_N+h}\right)\delta v_E+\frac{v_N}{R_M+h}\delta v_D-f_D{\varphi }_E+f_E{\varphi }_D+\delta f_N\end{array}\\ & \delta {\dot{v}}_E=& & =\left[\frac{2{\omega }_e(v_N\cos \phi -v_D\sin \phi )}{R_M+h}+\frac{v_N{v}_E{\sec }^2\phi }{(R_M+h)(R_N+h)}\right]\delta r_N+\frac{v_E{v}_D+v_N{v}_E\tan \phi }{(R_N+h{)}^2}\delta r_D\\ & & & +\left(2{\omega }_e\sin \phi +\frac{v_E\tan \phi }{R_N+h}\right)\delta v_N+\frac{v_D+v_N\tan \phi }{R_N+h}\delta v_E+\left(2{\omega }_e\cos \phi +\frac{v_E}{R_N+h}\right)\delta v_D\\ & & & +f_D{\varphi }_N-f_N{\varphi }_D+\delta f_E\\ & \delta {\dot{v}}_D=& & \begin{array}{r}\frac{2{\omega }_e{v}_E\sin \phi }{R_M+h}\delta r_N-\left[\frac{v_E^2}{(R_N+h{)}^2}+\frac{v_N^2}{(R_M+h{)}^2}-\frac{2g_p}{\sqrt{R_M{R}_N}+h}\right]\delta r_D-\frac{2v_N}{R_M+h}\delta v_N\end{array}\\ & & & \begin{array}{rl}& -2\left({\omega }_e\cos \phi +\frac{v_E}{R_N+h}\right)\delta v_E-f_E{\varphi }_N+f_N{\varphi }_E+\delta f_D\end{array}\end{array}$$
位置误差微分方程（e系推到转换为n系）：
$$\begin{array}{r}\delta {\dot{\mathbf{r}}}^n=-{\mathbf{\omega }}_{en}^n\times \delta {\mathbf{r}}^n+\delta \mathbf{\theta }\times {\mathbf{v}}^n+\delta {\mathbf{v}}^n\end{array}$$
展开：
$$\begin{array}{rl}& \delta {\dot{r}}_N=-\frac{v_D}{R_M+h}\delta r_N+\frac{v_N}{R_M+h}\delta r_D+\delta v_N\\ & \delta {\dot{r}}_E=\frac{v_E\tan \phi }{R_N+h}\delta r_N-\frac{v_D+v_N\tan \phi }{R_N+h}\delta r_E+\frac{v_E}{R_N+h}\delta r_D+\delta v_E\\ & \delta {\dot{r}}_D=\delta v_D\end{array}$$
IMU误差微分方程（建模为一阶高斯马尔可夫过程）：
$$\begin{array}{c}{\dot{\mathbf{b}}}_g(t)\begin{array}{r}=-\frac{1}{T_{qb}}{\mathbf{b}}_g(t)+{\mathbf{w}}_{gb}(t)\end{array}\\ {\dot{\mathbf{b}}}_a(t)=-\frac{1}{T_{gb}}{\mathbf{b}}_a(t)+{\mathbf{w}}_{ab}(t)\\ {\dot{\mathbf{s}}}_g(t)=-\frac{1}{T_{qs}}{\mathbf{s}}_g(t)+{\mathbf{w}}_{gs}(t)\\ {\dot{\mathbf{s}}}_a(t)=-\frac{1}{T_{as}}{\mathbf{s}}_a(t)+{\mathbf{w}}_{as}(t)\end{array}$$

GNSS/INS 松组合滤波设计

GNSS/INS 松组合卡尔曼滤波算法设计的关键在于：建立离散时间系统误差状态模型，然后构建 误差状态的线性观测方程。有了系统模型和观测方程则可直接使用卡尔曼滤波的基本方程进行组合导 航解算，即完成了组合导航的算法设计。GNSS/INS 松组合常采用误差状态卡尔曼滤波（间接卡尔曼滤波）进行组合导航解算，以解决系 统的非线性问题。

连续时间误差状态方程

卡尔曼滤波的状态向量包含导航状态误差和传感器误差，定义为：
$$\delta \mathbf{x}(t)={\left[\begin{array}{ccccccc}(\delta {\mathbf{r}}^n{)}^T& (\delta {\mathbf{v}}^n{)}^T& {\varphi }^T& {\mathbf{b}}_g^T& {\mathbf{b}}_a^T& {\mathbf{s}}_g^T& {\mathbf{s}}_a^T\end{array}\right]}^T$$
误差状态的连续时间微分方程：
$$\delta \dot{\mathbf{x}}(t)=\mathbf{F}(t)\delta \mathbf{x}(t)+\mathbf{G}(t)\mathbf{w}(t)$$
根据$ \delta\dot{\boldsymbol{x}}(t)$的表达式，容易写出连续时间系统噪声向量 $\mathbf{w}(t)、$状态转移矩阵$\mathbf{F}(t)$ 和噪声驱动矩阵 $\mathbf{G}(t)$。
$$\mathbf{w}={\left[\begin{array}{cccccc}{\mathbf{w}}_v^T& {\mathbf{w}}_{\varphi }^T& {\mathbf{w}}_{gb}^T& {\mathbf{w}}_{ab}^T& {\mathbf{w}}_{gs}^T& {\mathbf{w}}_{as}^T\end{array}\right]}^T$$

$${\mathbf{G}}_{21\times 18}=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ {\mathbf{C}}_b^n& 0& 0& 0& 0& 0\\ 0& {\mathbf{C}}_b^n& 0& 0& 0& 0\\ 0& 0& {\mathbf{I}}_{3\times 3}& 0& 0& 0\\ 0& 0& 0& {\mathbf{I}}_{3\times 3}& 0& 0\\ 0& 0& 0& 0& {\mathbf{I}}_{3\times 3}& 0\\ 0& 0& 0& 0& 0& {\mathbf{I}}_{3\times 3}\end{array}\right]$$

$$\mathbf{F}=\left[\begin{array}{ccccccc}{\mathbf{F}}_{rr}& {\mathbf{I}}_{3\times 3}& 0& 0& 0& 0& 0\\ {\mathbf{F}}_{vr}& {\mathbf{F}}_{vv}& [({\mathbf{C}}_b^n{\mathbf{f}}^b)\times ]& 0& {\mathbf{C}}_b^n& 0& {\mathbf{C}}_b^n\mathrm{diag}({\mathbf{f}}^b)\\ {\mathbf{F}}_{\varphi r}& {\mathbf{F}}_{\varphi v}& -({\mathbf{\omega }}_h^n\times )& -{\mathbf{C}}_b^n& 0& -{\mathbf{C}}_b^n\mathrm{diag}({\mathbf{\omega }}_h^b)& 0\\ 0& 0& 0& \frac{-1}{T_{gb}}{\mathbf{I}}_{3\times 3}& 0& 0& 0\\ & & & & & & \\ 0& 0& 0& 0& \frac{-1}{T_{ab}}{\mathbf{I}}_{3\times 3}& 0& 0\\ & & & & & & \\ 0& 0& 0& 0& 0& \frac{-1}{T_{gs}}{\mathbf{I}}_{3\times 3}& 0\\ & & & & & & \\ 0& 0& 0& 0& 0& 0& \frac{-1}{T_{as}}{\mathbf{I}}_{3\times 3}\end{array}\right]$$

状态方程离散化

误差状态的离散时间微分方程：
$$\delta x_k={\Phi }_{k/k-1}\delta x_{k-1}+w_{k-1}$$
其中，
$$\begin{array}{c}{\Phi }_{k/k-1}=\exp \left({\int }_{t_{k-1}}^{t_k}\mathbf{F}(t)dt\right)\\ {\mathbf{w}}_{k-1}={\int }_{t_{k-1}}^{t_k}{\mathbf{\Phi }}_{k/t}\mathbf{G}(t)\mathbf{w}(t)dt\end{array}$$
对状态转移矩阵进行近似化处理：

记离散化时间间隔$\Delta t=t_k-t_{k-1}$，当 $\mathbf{F}(t)$ 在较短的积分区间$[t_{k-1},t_k]$ 内变化不太剧烈时，且 设$\mathbf{F}(t_{k-1})\Delta t\ll \mathbf{I}$，则，
Φ k / k − 1 = exp ⁡ { F ( t k − 1 ) Δ t } ≈ I + F ( t k − 1 ) Δ t \mathbf{\Phi}_{k/k-1}=\exp\{\mathbf{F}(t_{k-1})\Delta t\}\approx\mathbf{I}+\mathbf{F}(t_{k-1})\Delta t Φk/k−1​=exp{ F(tk−1​)Δt}≈I+F(tk−1​)Δt
预测噪声方差阵：
$$\begin{array}{rl}{\mathbf{Q}}_k& ={\int }_{l_{k-1}}^{t_k}{\mathbf{\Phi }}_{k/t}\mathbf{G}(t)\mathbf{q}(t){\mathbf{G}}^T(t){\mathbf{\Phi }}_{k/t}^{\mathrm{T}}dt\end{array}$$
其中,
$${\mathbf{q}}_{18\times 18}(t)=\left[\begin{array}{cccccc}{\mathrm{VRW}}^2{\mathbf{I}}_{3\times 3}& 0& 0& 0& 0& 0\\ 0& {\mathrm{ARW}}^2{\mathbf{I}}_{3\times 3}& 0& 0& 0& 0\\ 0& 0& \frac{2{\sigma }_{gb}^2}{T_{gb}}{\mathbf{I}}_{3\times 3}& 0& 0& 0\\ 0& 0& 0& \frac{2{\sigma }_{ab}^2}{T_{ab}}{1}_{3\times 3}& 0& 0\\ 0& 0& 0& 0& \frac{2{\sigma }_{gs}^2}{T_{gs}}{\mathbf{I}}_{3\times 3}& 0\\ 0& 0& 0& 0& 0& \frac{2{\sigma }_{as}^2}{T_{as}}{\mathbf{I}}_{3\times 3}\end{array}\right]$$
当$\mathrm{G}(t)$在较短的积分区间$[t_{k-1},t_k]$内变化不太剧烈时，${\mathbf{Q}}_k$ 可简化为梯形积分。
$${\mathbf{Q}}_k\approx \frac{1}{2}\left[{\mathbf{\Phi }}_{k/k-1}\mathbf{G}(t_{k-1})\mathbf{q}(t_{k-1}){\mathbf{G}}^{\mathrm{T}}(t_{k-1}){\mathbf{\Phi }}_{k/k-1}^{\mathrm{T}}+\mathbf{G}(t_k)\mathbf{q}(t_k){\mathbf{G}}^{\mathrm{T}}(t_k)\right]\Delta t$$
离散状态误差预测过程：
$${\mathbf{x}}_{k/k-1}={\Phi }_{k/k-1}{\mathbf{x}}_{k-1}$$
状态协方差矩阵：
$${\mathbf{P}}_{k/k-1}={\Phi }_{k/k-1}{\mathbf{P}}_{k-1}{\mathbf{\Phi }}_{k/k-1}^T+Q_k$$

// 构造状态转移矩阵
// 位置误差
temp.setZero();
temp(0, 0)                = -pvapre_.vel[2] / rmh;
temp(0, 2)                = pvapre_.vel[0] / rmh;
temp(1, 0)                = pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh;
temp(1, 1)                = -(pvapre_.vel[2] + pvapre_.vel[0] * tan(pvapre_.pos[0])) / rnh;
    temp(1, 2)                = pvapre_.vel[1] / rnh;
    F.block(P_ID, P_ID, 3, 3) = temp;
    F.block(P_ID, V_ID, 3, 3) = Eigen::Matrix3d::Identity();

// 速度误差
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;
temp.setZero();
temp(0, 0)                  = pvapre_.vel[2] / rmh;
temp(0, 1)                  = -2 * (WGS84_WIE * sin(pvapre_.pos[0]) + pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh);
temp(0, 2)                  = pvapre_.vel[0] / rmh;
temp(1, 0)                  = 2 * WGS84_WIE * sin(pvapre_.pos[0]) + pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh;
temp(1, 1)                  = (pvapre_.vel[2] + pvapre_.vel[0] * tan(pvapre_.pos[0])) / rnh;
temp(1, 2)                  = 2 * WGS84_WIE * cos(pvapre_.pos[0]) + pvapre_.vel[1] / rnh;
temp(2, 0)                  = -2 * pvapre_.vel[0] / rmh;
temp(2, 1)                  = -2 * (WGS84_WIE * cos(pvapre_.pos(0)) + pvapre_.vel[1] / rnh);
F.block(V_ID, V_ID, 3, 3)   = temp;
F.block(V_ID, PHI_ID, 3, 3) = Rotation::skewSymmetric(pvapre_.att.cbn * accel);
F.block(V_ID, BA_ID, 3, 3)  = pvapre_.att.cbn;
F.block(V_ID, SA_ID, 3, 3)  = pvapre_.att.cbn * (accel.asDiagonal());

// 姿态误差
temp.setZero();
temp(0, 0) = -WGS84_WIE * sin(pvapre_.pos[0]) / rmh;
temp(0, 2) = pvapre_.vel[1] / rnh / rnh;
temp(1, 2) = -pvapre_.vel[0] / rmh / rmh;
temp(2, 0) = -WGS84_WIE * cos(pvapre_.pos[0]) / rmh - pvapre_.vel[1] / rmh / rnh / pow(cos(pvapre_.pos[0]), 2);
temp(2, 2) = -pvapre_.vel[1] * tan(pvapre_.pos[0]) / rnh / rnh;
F.block(PHI_ID, P_ID, 3, 3) = temp;
temp.setZero();
temp(0, 1)                    = 1 / rnh;
temp(1, 0)                    = -1 / rmh;
temp(2, 1)                    = -tan(pvapre_.pos[0]) / rnh;
F.block(PHI_ID, V_ID, 3, 3)   = temp;
F.block(PHI_ID, PHI_ID, 3, 3) = -Rotation::skewSymmetric(wie_n + wen_n);
F.block(PHI_ID, BG_ID, 3, 3)  = -pvapre_.att.cbn;
F.block(PHI_ID, SG_ID, 3, 3)  = -pvapre_.att.cbn * (omega.asDiagonal());

// IMU零偏误差和比例因子误差，建模成一阶高斯-马尔科夫过程
F.block(BG_ID, BG_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();
F.block(BA_ID, BA_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();
F.block(SG_ID, SG_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();
F.block(SA_ID, SA_ID, 3, 3) = -1 / options_.imunoise.corr_time * Eigen::Matrix3d::Identity();

// 系统噪声驱动矩阵
G.block(V_ID, VRW_ID, 3, 3)    = pvapre_.att.cbn;
G.block(PHI_ID, ARW_ID, 3, 3)  = pvapre_.att.cbn;
G.block(BG_ID, BGSTD_ID, 3, 3) = Eigen::Matrix3d::Identity();
G.block(BA_ID, BASTD_ID, 3, 3) = Eigen::Matrix3d::Identity();
G.block(SG_ID, SGSTD_ID, 3, 3) = Eigen::Matrix3d::Identity();
G.block(SA_ID, SASTD_ID, 3, 3) = Eigen::Matrix3d::Identity();

// 状态转移矩阵
Phi.setIdentity();
Phi = Phi + F * imucur.dt;
// 计算系统传播噪声
Qd = G * Qc_ * G.transpose() * imucur.dt;
Qd = (Phi * Qd * Phi.transpose() + Qd) / 2;
// 传播系统协方差和误差状态
Cov_ = Phi * Cov_ * Phi.transpose() + Qd;
dx_ = Phi * dx_;

GNSS位置观测方程

将IMU中心与GNSS天线相位中心对其：
$$r_G=r_I+{\mathrm{D}}_R^{-1}{\mathrm{C}}_b^n{l}^b$$
考虑各导航状态的误差：
$${\hat{r}}_G={\hat{r}}_I+{\mathrm{D}}_R^{-1}{\hat{\mathrm{C}}}_b^n{l}^b$$
引入扰动误差：
$$\begin{array}{c}{\hat{\mathbf{r}}}_G={\mathbf{r}}_I+{\mathbf{D}}_R^{-1}\delta {\mathbf{r}}^n+{\mathbf{D}}_R^{-1}\left[\mathbf{I}-(\mathbf{\varphi }\times )\right]{\mathbf{C}}_b^n{\mathbf{l}}_b^b\\ ={\mathbf{r}}_G+{\mathbf{D}}_R^{-1}\delta {\mathbf{r}}^n+{\mathbf{D}}_R^{-1}\left[({\mathbf{C}}_b^n{\mathbf{l}}^b)\times \varphi \right]\end{array}$$
GNSS 定位解算得到的 GNSS 天线相位中心的位置表示为：
$${\tilde{r}}_G=r_G-{\mathbf{D}}_R^{-1}{\mathbf{n}}_r$$
观测向量表示为 INS 推算的位置与 GNSS 位置观测之差,
$$\begin{array}{rl}\delta {\mathbf{z}}_r& ={\mathbf{D}}_R\left({\hat{\mathbf{r}}}_G^n-{\tilde{\mathbf{r}}}_G^n\right)\\ & \approx \delta {\mathbf{r}}^n+({\mathbf{C}}_b^n{\mathbf{l}}^b)\times \varphi +{\mathbf{n}}_r\end{array}$$
改写作如下形式：
$$\delta {\mathbf{z}}_r={\mathbf{H}}_r\delta \mathbf{x}+{\mathbf{n}}_r$$
观测矩阵为：
$${\mathbf{H}}_r=\left[\begin{array}{cccccc}{\mathbf{I}}_3& 0_3& ({\mathbf{C}}_b^n{\mathbf{l}}^b\times )& 0_3& 0_3& 0_3\end{array}\right]$$
测量噪声方差阵为：
$${\mathbf{R}}_k={\mathbf{n}}_r$$
卡尔曼增益：
$${\mathbf{K}}_k={\mathbf{P}}_{k/k-1}{H}_k^T(H_k{\mathbf{P}}_{k/k-1}{H}_k^T+R_k{)}^{-1}$$
最优估计状态：
$${\mathbf{x}}_k={\mathbf{x}}_{k/k-1}+{\mathbf{K}}_k(z_k-H_k{\mathbf{x}}_{k/k-1})$$
更新状态协方差矩阵：
$${\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k{H}_k){\mathbf{P}}_{k/k-1}(\mathbf{I}-{\mathbf{K}}_k{H}_k{)}^T+{\mathbf{K}}_k{R}_k{\mathbf{K}}_k^T$$

// GNSS位置观测向量
dz = Dr * (antenna_pos - gnssdata.blh);
// 构造GNSS位置观测矩阵
H_gnsspos.setZero();
H_gnsspos.block(0, P_ID, 3, 3) = Eigen::Matrix3d::Identity();
H_gnsspos.block(0, PHI_ID, 3, 3) =
Rotation::skewSymmetric(pvacur_.att.cbn * options_.antlever);
// 位置观测噪声阵
R_gnsspos =
gnssdata.std.cwiseProduct(gnssdata.std).asDiagonal();
kf-gins/gi_engine.cpp/GIEngine::EKFUpdate
// 计算Kalman增益
temp = H * Cov_ * H.transpose() + R;
K = Cov_ * H.transpose() * temp.inverse();
// 更新系统误差状态和协方差
I = I - K * H;
dx_ = dx_ + K * (dz - H * dx_);
Cov_ = I * Cov_ * I.transpose() + K * R * K.transpose();

误差反馈

按照扰动形式进行导航误差反馈，估计的IMU残差更新到IMU误差上
$$\begin{array}{rlrl}& \mathbf{r}=\hat{\mathbf{r}}-{\mathbf{D}}_r\delta {\mathbf{r}}^n& & {\overline{\mathbf{b}}}_g\leftarrow {\overline{\mathbf{b}}}_g+{\mathbf{b}}_g,{\overline{\mathbf{b}}}_a\leftarrow {\overline{\mathbf{b}}}_a+{\mathbf{b}}_a\\ & {\mathbf{v}}^n={\hat{\mathbf{v}}}^n-\delta {\mathbf{v}}^n& & {\overline{\mathbf{s}}}_g\leftarrow {\overline{\mathbf{s}}}_g+{\mathbf{s}}_g,{\overline{\mathbf{s}}}_a\leftarrow {\overline{\mathbf{s}}}_a+{\mathbf{s}}_a\\ & {\mathbf{C}}_b^n=[\mathbf{I}+(\mathbf{\varphi }\times )]{\hat{\mathbf{C}}}_b^n& & \end{array}$$

// 位置、速度误差反馈
Eigen::Vector3d delta_r = dx_.block(P_ID, 0, 3, 1);
pvacur_.pos -= Dr_inv * delta_r;
vectemp = dx_.block(V_ID, 0, 3, 1);
pvacur_.vel -= vectemp;
// 姿态误差反馈
vectemp = dx_.block(PHI_ID, 0, 3, 1);
Eigen::Quaterniond qpn = Rotation::rotvec2quaternion(vectemp);
pvacur_.att.qbn = qpn * pvacur_.att.qbn;
// IMU零偏误差反馈
vectemp = dx_.block(BA_ID, 0, 3, 1);
imuerror_.accbias += vectemp;
// 误差状态反馈到系统状态后,将误差状态清零
dx_.setZero();

oldsymbol{s}_a\&\boldsymbol{C}_b^{n=[\boldsymbol{I}+(\boldsymbol{\phi}\times)]\hat{\boldsymbol{C}}_b}n\end{aligned}
$$

// 位置、速度误差反馈
Eigen::Vector3d delta_r = dx_.block(P_ID, 0, 3, 1);
pvacur_.pos -= Dr_inv * delta_r;
vectemp = dx_.block(V_ID, 0, 3, 1);
pvacur_.vel -= vectemp;
// 姿态误差反馈
vectemp = dx_.block(PHI_ID, 0, 3, 1);
Eigen::Quaterniond qpn = Rotation::rotvec2quaternion(vectemp);
pvacur_.att.qbn = qpn * pvacur_.att.qbn;
// IMU零偏误差反馈
vectemp = dx_.block(BA_ID, 0, 3, 1);
imuerror_.accbias += vectemp;
// 误差状态反馈到系统状态后,将误差状态清零
dx_.setZero();

感谢武汉大学卫星导航定位技术研究中心多源智能导航实验室(i2Nav)牛小骥教授团队开源的KF-GINS软件平台