ignav卡尔曼滤波中的待估计参数

由下面代码可以看出ignav卡尔曼滤波中待估计的状态：[attitude、velocity、position、accl bias、gyro bias、... ]^T

 

extern int xiA (const insopt_t *opt) {return 0;}    /* attitude states index */
extern int xiV (const insopt_t *opt) {return 3;}    /* velocity states index */
extern int xiP (const insopt_t *opt) {return 6;}    /* position states index */
extern int xiBa(const insopt_t *opt) {return 9;}    /* accl bias state index */

/* get index of gyro bias states---------------------------------------------*/
extern int xiBg(const insopt_t *opt) {return xnA(opt)+xnV(opt)+xnP(opt)+xnBa(opt);}

/* get index of time synchronization error for IMU and GNSS user equipment---*/
extern int xiDt(const insopt_t *opt)
{
    return xnA(opt)+xnV(opt)+xnP(opt)+xnBa(opt)+xnBg(opt);
}
/* get index of residual scale factors of gyroscopes-------------------------*/
extern int xiSg(const insopt_t *opt)
{
    return xnA(opt)+xnV(opt)+xnP(opt)+xnBa(opt)+xnBg(opt)+xnDt(opt);
}
/* get index of residual scale factors of accl state-------------------------*/
extern int xiSa(const insopt_t *opt)
{
    return xnA(opt)+xnV(opt)+xnP(opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt);
}
/* get index of non-orthogonal between sensor axes for gyro states-----------*/
extern int xiRg(const insopt_t *opt)
{
    return xnA (opt)+xnV(opt)+xnP(opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt);
}
/* get index of non-orthogonal between sensor axes for accl states-----------*/
extern int xiRa(const insopt_t *opt)
{
    return xnA (opt)+xnV (opt)+xnP(opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt);
}
/* get index lever arm states------------------------------------------------*/
extern int xiLa(const insopt_t *opt)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt);
}
/* get index of odometry scale factor states---------------------------------*/
extern int xiOs(const insopt_t *opt)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt);
}
/* get index of odometry lever arm states------------------------------------*/
extern int xiOl(const insopt_t *opt)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt);
}
/* get index of odometry misalignment of body frame and rear frame states----*/
extern int xiOa(const insopt_t *opt)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt)+xnOl(opt);
}
/* get index of misalignment of camera to imu body---------------------------*/
extern int xiCm(const insopt_t *opt)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt)+xnOl(opt)+xnOa(opt);
}
/* get index of misalignment of b-frame to v-frame---------------------------*/
extern int xiVm(const insopt_t *opt)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt)+xnOl(opt)+xnOa(opt)+
           xnCm(opt);
}
/* get index of receiver clock bias state------------------------------------*/
extern int xiRc(const insopt_t *opt)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt)+xnOl(opt)+xnOa(opt)+
           xnCm(opt)+xnVm(opt);
}
/* get index of receiver clock drift-----------------------------------------*/
extern int xiRr(const insopt_t *opt)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt)+xnOl(opt)+xnOa(opt)+
           xnCm(opt)+xnVm(opt)+xnRc(opt);
}
/* get index of ionos (s:satellite no)---------------------------------------*/
extern int xiIo(const insopt_t *opt,int s)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt)+xnOl(opt)+xnOa(opt)+
           xnCm(opt)+xnVm(opt)+xnRc(opt)+xnRr(opt)+(s-1);
}
/* get index of tropos (r:0=rov,1:ref)---------------------------------------*/
extern int xiTr(const insopt_t *opt,int r)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt)+xnOl(opt)+xnOa(opt)+
           xnCm(opt)+xnVm(opt)+xnRc(opt)+xnRr(opt)+xnI (opt)+xnT (opt)/2*r;
}
/* get index of receiver h/w bias--------------------------------------------*/
extern int xiLl(const insopt_t *opt,int f)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt)+xnOl(opt)+xnOa(opt)+
           xnCm(opt)+xnVm(opt)+xnRc(opt)+xnRr(opt)+xnI (opt)+xnT (opt)+f;
}
/* get index of L5-receiver-dcb ---------------------------------------------*/
extern int xiDl(const insopt_t *opt)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt)+xnOl(opt)+xnOa(opt)+
           xnCm(opt)+xnVm(opt)+xnRc(opt)+xnRr(opt)+xnI (opt)+xnT (opt)+xnL (opt);
}
/* get index of phase bias (s:satno,f:freq)----------------------------------*/
extern int xiBs(const insopt_t *opt,int s,int f)
{
    return xnA (opt)+xnV (opt)+xnP (opt)+xnBa(opt)+xnBg(opt)+xnDt(opt)+xnSg(opt)+
           xnSa(opt)+xnRg(opt)+xnRa(opt)+xnLa(opt)+xnOs(opt)+xnOl(opt)+xnOa(opt)+
           xnCm(opt)+xnVm(opt)+xnRc(opt)+xnRr(opt)+xnI (opt)+xnT (opt)+xnL (opt)+
           xnD(opt)+MAXSAT*f+s-1;
}