DGNSS 定位算法实现

//step 4.2 DGNSS 定位 TOBS coef, coef_base, coef_td; // 定义观测值结构体变量coef、coef_base、coef_td TOBS sd_coef_last_epoch; // 上一历元的单差观测值结构体变量 // 准备零差分线性化观测值 if (linearize_zd(coef, pLastObs, &obss, &navs, tropmodel, ionomodel, frqs, ChannelUsed, true, true, true)) { // 准备基准观测值 bool bReadRecordBase = true; TOBS* pLastObs_base = obss_base.GetLast(); if (pLastObs_base) { if (pLastObs_base->t.GetT_ms() >= t_end_ms) { bReadRecordBase = false; } } if (bReadRecordBase) { obss_base.ReadOneRecord(t_old * 1e-3); } pLastObs_base = obss_base.GetLast(); if (pLastObs_base) { // 获取GLONASS频率编号 pLastObs_base->GetMeasFrqNum(&navs); // 时间检查 if (pLastObs_base->t.GetT_ms() == t_end_ms) { // 初始化 if (sitexyz_base[0] == 0) { if (single_epoch_spp(*pLastObs_base, &navs, tropmodel, ionomodel)) { double deltapos[3] = { 0 }; gDotDif(deltapos, pLastObs_base->pos, obss_base.m_header.m_PosAntenna, 3); if (gNorm(deltapos, 3) > 30) { cout << 'change the base header pos since it is inaccurate!' << endl; obss_base.m_header.SetAntennaPos(pLastObs_base->pos); } obss_base.m_header.GetStonePos(pLastObs_base->pos); } memcpy(sitexyz_base, pLastObs_base->pos, 3 * g_sz_dbl); memcpy(sitexyz_base + 3, pLastObs_base->std_pos, 3 * g_sz_dbl); } else { single_epoch_spp(*pLastObs_base, &navs, tropmodel, ionomodel); obss_base.m_header.SetStonePos(sitexyz_base); obss_base.m_header.GetAntennaPos(pLastObs_base->pos); // 每个历元都修正基准位置 } obss_base.m_header.GetAntennaPos(pLastObs_base->pos); // 每个历元都修正基准位置 pLastObs_base->RemoveLowElv(cutoff); // 准备基准的零差分线性化观测值 if (linearize_zd(coef_base, pLastObs_base, &obss_base, &navs, tropmodel, ionomodel, frqs, ChannelUsed, true, true, true)) { // 准备站间单差观测值 TOBS coef_sd; if (linearize_sd(coef_sd, coef, coef_base, frqs, ChannelUsed)) { // 准备双差观测值 TOBS coef_dd; if (linearize_dd(coef_dd, coef_sd, sd_coef_last_epoch, sysused, frqs, ChannelUsed)) { // 双差定位 if (single_epoch_dd_code(coef_dd, coef_sd, frqs)) { // 将状态量复制到头文件 COBSSET::CopyStateToHeader(obss.m_header, coef_dd); // 输出 double sitexyz[3] = { 0 }; obss.m_header.SetAntennaPos(coef_dd.pos); obss.m_header.GetStonePos(sitexyz);

                        // 输出SPP位置
                        CCrd posspp; posspp.SetXYZ(sitexyz);
                        double xyh[6] = { 0 };
                        double blh[3];
                        posspp.GetXYH(xyh);
                        posspp.GetBLH(blh);

                        Matrix Qxyz(3, 3);

                        Qxyz(0, 0) = coef_dd.std_pos[0] * coef_dd.std_pos[0];
                        Qxyz(1, 1) = coef_dd.std_pos[1] * coef_dd.std_pos[1];
                        Qxyz(2, 2) = coef_dd.std_pos[2] * coef_dd.std_pos[2];

                        Matrix Rxyz2enu = gXYZ2NEU_R(blh[0], blh[1]);
                        Matrix Qenu = Rxyz2enu * Qxyz * ~Rxyz2enu;
                        xyh[3] = sqrt(Qenu(0, 0));
                        xyh[4] = sqrt(Qenu(1, 1));
                        xyh[5] = sqrt(Qenu(2, 2));

                        char buff[1025] = { 0 };
                        snprintf(buff, 1024,
                            "%8.2f\t%3d\t%8.2f\t%8.4f\t%8.4f\t%8.4f\t%8.4f\t%8.4f\t%8.4f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\n",
                            pLastObs->t.GetTow(), pLastObs->GetCnt(), pLastObs->GetPDOP(),
                            sitexyz[0], sitexyz[1], sitexyz[2],
                            xyh[0], xyh[1], xyh[2],
                            coef_dd.std_pos[0], coef_dd.std_pos[1], coef_dd.std_pos[2],
                            xyh[3], xyh[4], xyh[5]);
                        fprintf(fpOut, buff);

                        cout << buff;
                    }
                }
                sd_coef_last_epoch = coef_sd;
            } // 如果站间单差观测值准备成功
        } // 如果基准的零差分线性化观测值准备成功
    } // 如果基准观测值的时间等于历元结束时间
} // 如果存在基准观测值

} // 如果零差分线性化观测值准备成功

// 释放资源 if (fpOut) { fclose(fpOut); fpOut = NULL; }

return 1;