程序的坐标解算结果为nan
如图:
发现从第二个历元开始,卡尔曼滤波那里的第二个;历元传递进来的Qwk就不对
//Time update
VectorXd Xkk_1 = F*tXk_1,Vk;
MatrixXd Pkk_1 = F*tPk_1*F.transpose() + Qwk,I,tempKB,Kk;
//Calculated gain matrix
Kk = (Pkk_1*Bk.transpose())*((Bk*Pkk_1*Bk.transpose() + Rk).inverse());
//Filter update
Vk = Lk - Bk*Xkk_1;
Qwk的第七颗卫星的单差电离层延迟显示3.09531e+251,因此后续的Pkk_1 以至于Kk 和结果计算出现了nan。
进行单差电离层延迟计算时用到了时间间隔:
for(int i = m_const_param; i < m_const_param+epochLenLB;i++)
{
double fact = qCos(currEpoch.at(i-m_const_param).RoverStation.EA[0]*M_PI/180);
//历元间时间间隔
double timeSep = currEpoch.at(i-m_const_param).RoverStation.UTCTime.interval;
m_Qwk_1(i,i) = qPow(0.001*currEpoch.at(i-m_const_param).RoverStation.UTCTime.baselineLen/1e4*fact,2)*timeSep;
}// for ION
其中:
double timeSep = currEpoch.at(i-m_const_param).RoverStation.UTCTime.interval;
问题出现在currEpoch.at(i-m_const_param).RoverStation.UTCTime.interval上,某一颗卫星的interval是类似于3.09531e+251的数值。
但是在前面我进行的interval相关操作如下,进行qDebug()输出却没有问题,包括m_interval_r和tempSatlitData_r.UTCTime.interval都输出了,是正常的时间间隔。
//历元间时时间隔
if(m_interval_r >0)
tempSatlitData_r.UTCTime.interval = m_interval_r;
else if(multepochSatlitData_r.length()>1)
{
double GPST1 = qCmpGpsT.YMD2GPSTime(multepochSatlitData_r[0].at(0).UTCTime.Year,multepochSatlitData_r[0].at(0).UTCTime.Month,multepochSatlitData_r[0].at(0).UTCTime.Day,
multepochSatlitData_r[0].at(0).UTCTime.Hours,multepochSatlitData_r[0].at(0).UTCTime.Minutes,multepochSatlitData_r[0].at(0).UTCTime.Seconds);
double GPST2 = qCmpGpsT.YMD2GPSTime(multepochSatlitData_r[1].at(0).UTCTime.Year,multepochSatlitData_r[1].at(0).UTCTime.Month,multepochSatlitData_r[1].at(0).UTCTime.Day,
multepochSatlitData_r[1].at(0).UTCTime.Hours,multepochSatlitData_r[1].at(0).UTCTime.Minutes,multepochSatlitData_r[1].at(0).UTCTime.Seconds);
m_interval_r = abs(GPST2-GPST1);
}
else
{
QString errorline;
ErrorMsg(errorline);
tempSatlitData_r.badMsg.append("RoverStation invalid epoch interval"+errorline);
tempSatlitData_r.UTCTime.interval = 0;
}
最后没办法我将代码改为了
//历元间时间间隔
double timeSep = currEpoch.at(0).RoverStation.UTCTime.interval;
有人遇到类似的问题吗,请求指导。