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


    由下面代码可以看出ignav卡尔曼滤波中待估计的状态:[attitudevelocitypositionaccl 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;
    }
  • 相关阅读:
    java_设计模式_观察者模式_Observer Pattern(2016-07-27)
    java_设计模式_策略模式_Strategy pattern(2016-07-15)
    一个简单的路由,用javascript实现
    sublime 编辑器配置和构建检查
    图文列表的图片居中
    不用css3的响应式img(按比例缩小图片)
    做前端的小笔记
    javascript创建跟随鼠标好玩的东西
    几十行代码就搞定俄罗斯方块
    来看看css3中的box-shadow
  • 原文地址:https://www.cnblogs.com/y-z-h/p/13904221.html
Copyright © 2020-2023  润新知