Open LucaFibbi opened 4 years ago
XiaoGongWei
commented
4 years ago Dear LucaFibbi ,
LucaFibbi
commented
4 years ago I have used and use the latest version of MG-APP. I have check which variables are the cause of the assert. In function QKalmanFilter::KalmanforStatic(MatrixXd Bk,VectorXd Lk,MatrixXd F,MatrixXd Qwk,MatrixXd Rk,VectorXd &tXk_1,MatrixXd &tPk_1) the abort is raise at the statment: Kk = (Pkk_1Bk.transpose())((BkPkk_1Bk.transpose() + Rk).inverse()); The variable with wrong size is Pkk_1, that it is calculated from F, tPk_1 and Qwk: Pkk_1 = FtPk_1F.transpose() + Qwk To understand the origin of the problem I printed the values of the variable m_Fk_1 (m_Fk_1=F) in every place where a change in its size could take place. I used only m_Fk_1 because the other variables have equal behavior. Here the change at the code for print the debug messages:
diff --git a/QKalmanFilter.cpp b/QKalmanFilter.cpp
--- a/QKalmanFilter.cpp
+++ b/QKalmanFilter.cpp
@@ -81,11 +81,13 @@ void QKalmanFilter::initKalman(QVector< SatlitData > &currEpoch,MatrixXd &B,Vect
case KALMAN_MODEL::SPP_STATIC:
case KALMAN_MODEL::SPP_KINEMATIC:
m_Fk_1.resize(m_const_param, m_const_param);
+std::cout << "DEBUG QKalmanFilter::" << __func__ << ":"<< __LINE__ << " m_Fk_1 size " << m_Fk_1.rows()<<"x"<<m_Fk_1.cols() << std::endl;
m_Fk_1.setIdentity(m_const_param, m_const_param);
break;
case KALMAN_MODEL::PPP_KINEMATIC:
case KALMAN_MODEL::PPP_STATIC:
m_Fk_1.resize(m_const_param+epochLenLB,m_const_param+epochLenLB);
+std::cout << "DEBUG QKalmanFilter::" << __func__ << ":"<< __LINE__ << " m_Fk_1 size " << m_Fk_1.rows()<<"x"<<m_Fk_1.cols() << std::endl;
m_Fk_1.setIdentity(m_const_param+epochLenLB,m_const_param+epochLenLB);
break;
default:
@@ -168,6 +170,7 @@ void QKalmanFilter::changeKalmanPara( QVector< SatlitData > &epochSatlitData,QVe
int epochLenLB = epochSatlitData.length();
m_Fk_1.resize(m_const_param+epochLenLB,m_const_param+epochLenLB);
+std::cout << "DEBUG QKalmanFilter::" << __func__ << ":"<< __LINE__ << " m_Fk_1 size " << m_Fk_1.rows()<<"x"<<m_Fk_1.cols() << std::endl;
m_Fk_1.setZero();
m_Fk_1.setIdentity(m_const_param+epochLenLB,m_const_param+epochLenLB);
//Fk_1(4,4) = 0;//Static PPP has only a clock difference of 0
@@ -266,6 +269,11 @@ void QKalmanFilter::KalmanforStatic(MatrixXd Bk,VectorXd Lk,MatrixXd F,MatrixXd
VectorXd Xkk_1 = F*tXk_1,Vk;
MatrixXd Pkk_1 = F*tPk_1*F.transpose() + Qwk,I,tempKB,Kk;
//Calculated gain matrix
+std::cout << "DEBUG QKalmanFilter::" << __func__ << ":"<< __LINE__ << " Qwk size " << Qwk.rows() << "x" << Qwk.cols() << std::endl;
+std::cout << "DEBUG QKalmanFilter::" << __func__ << ":"<< __LINE__ << " F size " << F.rows() << "x" << F.cols() << std::endl;
+std::cout << "DEBUG QKalmanFilter::" << __func__ << ":"<< __LINE__ << " tPk_1 size " << tPk_1.rows() << "x" << tPk_1.cols() << std::endl;
+std::cout << "DEBUG QKalmanFilter::" << __func__ << ":"<< __LINE__ << " Pkk_1 size " << Pkk_1.rows() << "x" << Pkk_1.cols() << std::endl;
+std::cout << "DEBUG QKalmanFilter::" << __func__ << ":"<< __LINE__ << " Bk size " << Bk.rows() << "x" << Bk.cols() << std::endl;
Kk = (Pkk_1*Bk.transpose())*((Bk*Pkk_1*Bk.transpose() + Rk).inverse());
//Filter update
Vk = Lk - Bk*Xkk_1;
@@ -580,6 +588,7 @@ bool QKalmanFilter::KalmanforStatic(QVector< SatlitData > &preEpoch,QVector< Sat
// restore filter state
m_Fk_1 = temp_Fk_1; m_Qwk_1 = temp_Qwk_1; m_Rk_1 = temp_Rk_1;
+std::cout << "DEBUG QKalmanFilter::" << __func__ << ":"<< __LINE__ << " m_Fk_1 size " << m_Fk_1.rows()<<"x"<<m_Fk_1.cols() << std::endl;
m_Pk_1 = temp_Pk_1; m_Xk_1 = temp_Xk_1;
memcpy(m_SPP_Pos, temp_SPP_POS, 3*sizeof(double));
@@ -621,6 +630,7 @@ bool QKalmanFilter::KalmanforStatic(QVector< SatlitData > &preEpoch,QVector< Sat
{
// restore filter state
m_Fk_1 = temp_Fk_1; m_Qwk_1 = temp_Qwk_1; m_Rk_1 = temp_Rk_1;
+std::cout << "DEBUG QKalmanFilter::" << __func__ << ":"<< __LINE__ << " m_Fk_1 size " << m_Fk_1.rows()<<"x"<<m_Fk_1.cols() << std::endl;
m_Pk_1 = temp_Pk_1; m_Xk_1 = temp_Xk_1;
memcpy(m_SPP_Pos, temp_SPP_POS, 3*sizeof(double));
X.setZero();Here the output for the two last epoch before the assert:
Epoch: 1217
GPST: 10:08:30
Sat. Number: 6
Valid Sat. Number: 6
Estimated coordinates: [ 4696989.2124,723994.9939,4239678.8065 ]
DEBUG QKalmanFilter::changeKalmanPara:173 m_Fk_1 size 10x10 <--- correct change the satellite number change from 6 to 5
DEBUG QKalmanFilter::KalmanforStatic:272 Qwk size 10x10
DEBUG QKalmanFilter::KalmanforStatic:273 F size 10x10
DEBUG QKalmanFilter::KalmanforStatic:274 tPk_1 size 10x10
DEBUG QKalmanFilter::KalmanforStatic:275 Pkk_1 size 10x10
DEBUG QKalmanFilter::KalmanforStatic:276 Bk size 10x10
DEBUG QKalmanFilter::KalmanforStatic:633 m_Fk_1 size 11x11 <--- incorrect change of the matrix m_Fk_1 the satellite number not change
Epoch: 1216
GPST: 10:08:00
Sat. Number: 5
Valid Sat. Number: 5
Estimated coordinates: [ 4696989.2124,723994.9939,4239678.8065 ]
DEBUG QKalmanFilter::KalmanforStatic:272 Qwk size 11x11
DEBUG QKalmanFilter::KalmanforStatic:273 F size 11x11
DEBUG QKalmanFilter::KalmanforStatic:274 tPk_1 size 11x11
DEBUG QKalmanFilter::KalmanforStatic:275 Pkk_1 size 11x11
DEBUG QKalmanFilter::KalmanforStatic:276 Bk size 10x10
MG_APP: ../MG_APP/resources/myeigen/Eigen/src/Core/ProductBase.h:102: Eigen::ProductBase<Derived, Lhs, Rhs>::ProductBase(const Lhs&, const Rhs&) [with Derived = Eigen::GeneralProduct<Eigen::Matrix<double, -1, -1>, Eigen::Matrix<double, -1, -1>, 5>; Lhs = Eigen::Matrix<double, -1, -1>; Rhs = Eigen::Matrix<double, -1, -1>]: asserzione "a_lhs.cols() == a_rhs.rows() && "invalid matrix product" && "if you wanted a coeff-wise or a dot product use the respective explicit functions"" non riuscita.From the previous output I understand. The number of satellite for the epoch with the assert is 5 The Qwk, F e tPk_1 have size of 11x11 while the correct value is 10x10 as Bk matrix. The Qwk, F e tPk_1 matrix takes 11x11 value of the size at QKalmanFilter::KalmanforStatic:633.
LucaFibbi
commented
4 years ago I have used cut-of-angle = 0. In your image I see cut-of-ange = 10. Maybe it is the cause because you no assert was found.
XiaoGongWei
commented
4 years ago Yes, the cutoff height Angle of the satellite is usually set to something greater than 0 (it can set it to 0 ,and Satellites are not delete by height Angle), usually 7.5 degree or 10 degree. If you set it to zero then all satellites are involved in the calculation. It's reasonable to set it to zero degree.
I will debug this problem, thank you.
LucaFibbi
commented
4 years ago OK, can't be reasonable to set cut of angle to zero degree, but the program not ignore all satellites. I did many tests with cut of angle equal to zero degree where MG_APP produced a result without the assert.
XiaoGongWei
commented
4 years ago You are right. I misunderstood. I will debug this problem, thank you.
XiaoGongWei
commented
4 years ago This bug has been fixed, the main problem is the following code. A new commit has been push. bf7405aadd8373e5318faa62e34abf2602c83d95
Change if(epochSatlitData.length() < m_minSatFlag to if(epochSatlitData.length() < m_minSatFlag || temp_spp_pos[0] == 0) in QPPPBackSmooth::Run 615 line.
add is_filter_good flag.
// store spp position
spp_vct[0] = temp_spp_pos[0]; spp_vct[1] = temp_spp_pos[1]; spp_vct[2] = temp_spp_pos[2];
if (!m_Solver_Method.compare("SRIF", Qt::CaseInsensitive))
is_filter_good = m_SRIFAlgorithm.SRIFforStatic(prevEpochSatlitData,epochSatlitData,spp_pos,X,P);
else
is_filter_good = m_KalmanClass.KalmanforStatic(prevEpochSatlitData,epochSatlitData,spp_pos,X,P);
//Save the last epoch satellite data
if(is_filter_good)
{
prevEpochSatlitData = epochSatlitData;
continue_bad_epoch = 0;
}
else
{
continue_bad_epoch++;
memset(spp_vct, 0, 3sizeof(double));
memset(spp_pos, 0, 3sizeof(double));
X.setZero();
}
assert_test_case.zip
With the attach test case the program MG_APP abort with a assert. The options used are: TropDeal=Hopfield(GPT2) Method=Kalman Cut-off Angle=0 Kinemati Yes Satellite system GPS PPP NoSmooth PPP Back Yes run PPP The program exit with the following error message:
MG_APP: ../MG_APP/resources/myeigen/Eigen/src/Core/ProductBase.h:102: Eigen::ProductBase<Derived, Lhs, Rhs>::ProductBase(const Lhs&, const Rhs&) [with Derived = Eigen::GeneralProduct<Eigen::Matrix<double, -1, -1>, Eigen::Matrix<double, -1, -1>, 5>; Lhs = Eigen::Matrix<double, -1, -1>; Rhs = Eigen::Matrix<double, -1, -1>]: Assertion `a_lhs.cols() == a_rhs.rows() && "invalid matrix product" && "if you wanted a coeff-wise or a dot product use the respective explicit functions"' failed.
When the error is raise, the program backtrace is:
0 __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
1 0x00007ffff53ef801 in __GI_abort () at abort.c:79
2 0x00007ffff53df39a in __assert_fail_base (fmt=0x7ffff7fb3b03 "%s%s%s:%u: %s%sasserzione \"%s\" non riuscita.\n%n",
3 0x00007ffff53df412 in __GI___assert_fail (
4 0x00005555555f7e0b in Eigen::ProductBase<Eigen::GeneralProduct<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 5>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >::ProductBase (
5 0x00005555555f702f in Eigen::GeneralProduct<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 5>::GeneralProduct (this=0x7fffffffb850, lhs=..., rhs=...)
6 0x00005555555f66cf in Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >::operator*<Eigen::Matrix<double, -1, -1, 0, -1, -1> > (this=0x7fffffffb9d0, other=...) at ../MG_APP/resources/myeigen/Eigen/src/Core/GeneralProduct.h:598
7 0x00005555555f2d8c in QKalmanFilter::KalmanforStatic (this=0x7fffffffca40, Bk=..., Lk=..., F=..., Qwk=..., Rk=..., tXk_1=...,
8 0x00005555555f5899 in QKalmanFilter::filter (this=0x7fffffffca40, preEpoch=..., currEpoch=..., X=..., P=...)
9 0x00005555555f4933 in QKalmanFilter::KalmanforStatic (this=0x7fffffffca40, preEpoch=..., currEpoch=...,
10 0x0000555555730a3e in QPPPBackSmooth::Run (this=0x7fffffffbf30, isDisplayEveryEpoch=true) at ../MG_APP/QPPPBackSmooth.cpp:633
11 0x00005555557116a9 in MainWindow::RunPPP (this=0x7fffffffdbe0) at ../MG_APP/mainwindow.cpp:303
12 0x000055555574ef0e in MainWindow::qt_static_metacall (_o=0x7fffffffdbe0, _c=QMetaObject::InvokeMetaMethod, _id=2,
13 0x00007ffff6398555 in QMetaObject::activate(QObject*, int, int, void**) () from /usr/lib/x86_64-linux-gnu/libQt5Core.so.5
14 0x00007ffff7564ba2 in QAbstractButton::clicked(bool) () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
15 0x00007ffff7564dba in ?? () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
16 0x00007ffff756619a in ?? () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
17 0x00007ffff756638d in QAbstractButton::mouseReleaseEvent(QMouseEvent*) () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
18 0x00007ffff74b2048 in QWidget::event(QEvent*) () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
19 0x00007ffff747383c in QApplicationPrivate::notify_helper(QObject, QEvent) () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
20 0x00007ffff747b65f in QApplication::notify(QObject, QEvent) () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
21 0x00007ffff63698d8 in QCoreApplication::notifyInternal2(QObject, QEvent) () from /usr/lib/x86_64-linux-gnu/libQt5Core.so.5
22 0x00007ffff747a632 in QApplicationPrivate::sendMouseEvent(QWidget, QMouseEvent, QWidget, QWidget, QWidget**, QPointer&, bool) () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
23 0x00007ffff74cd16b in ?? () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
24 0x00007ffff74cf7da in ?? () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
25 0x00007ffff747383c in QApplicationPrivate::notify_helper(QObject, QEvent) () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
26 0x00007ffff747b104 in QApplication::notify(QObject, QEvent) () from /usr/lib/x86_64-linux-gnu/libQt5Widgets.so.5
27 0x00007ffff63698d8 in QCoreApplication::notifyInternal2(QObject, QEvent) () from /usr/lib/x86_64-linux-gnu/libQt5Core.so.5
28 0x00007ffff6cb7583 in QGuiApplicationPrivate::processMouseEvent(QWindowSystemInterfacePrivate::MouseEvent*) ()
from /usr/lib/x86_64-linux-gnu/libQt5Gui.so.5
29 0x00007ffff6cb9055 in QGuiApplicationPrivate::processWindowSystemEvent(QWindowSystemInterfacePrivate::WindowSystemEvent*) ()
from /usr/lib/x86_64-linux-gnu/libQt5Gui.so.5
30 0x00007ffff6c902eb in QWindowSystemInterface::sendWindowSystemEvents(QFlags) ()
from /usr/lib/x86_64-linux-gnu/libQt5Gui.so.5
31 0x00007fffefbde260 in ?? () from /usr/lib/x86_64-linux-gnu/libQt5XcbQpa.so.5
32 0x00007ffff38de417 in g_main_context_dispatch () from /usr/lib/x86_64-linux-gnu/libglib-2.0.so.0
33 0x00007ffff38de650 in ?? () from /usr/lib/x86_64-linux-gnu/libglib-2.0.so.0
34 0x00007ffff38de6dc in g_main_context_iteration () from /usr/lib/x86_64-linux-gnu/libglib-2.0.so.0
35 0x00007ffff63c288f in QEventDispatcherGlib::processEvents(QFlags) ()
from /usr/lib/x86_64-linux-gnu/libQt5Core.so.5
36 0x00007ffff636790a in QEventLoop::exec(QFlags) () from /usr/lib/x86_64-linux-gnu/libQt5Core.so.5
37 0x00007ffff63709b4 in QCoreApplication::exec() () from /usr/lib/x86_64-linux-gnu/libQt5Core.so.5
38 0x0000555555605dc5 in main (argc=1, argv=0x7fffffffddb8) at ../MG_APP/main.cpp:18
The last normal output in the backward filter is:
Epoch: 1216 GPST: 10:08:00 Sat. Number: 5 Valid Sat. Number: 5 Estimated coordinates: [ 4696989.2124,723994.9939,4239678.8065 ]