fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 01:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

-added comments

Browse Source 
-removed unused print functions
-removed false _imu_time_last variable (correct is _time_last_imu)
This commit is contained in:
Roman
2016-02-14 09:49:26 +01:00
parent 9d7340e187
commit ce0ddc0207
7 changed files with 89 additions and 274 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
-33
View File
@@ -94,39 +94,6 @@ void Ekf::resetHeight()
}
}
#if defined(__PX4_POSIX) && !defined(__PX4_QURT)
void Ekf::printCovToFile(char const *filename)
{
std::ofstream myfile;
myfile.open(filename);
myfile << "Covariance matrix\n";
myfile << std::setprecision(1);
for (int i = 0; i < _k_num_states; i++) {
for (int j = 0; j < _k_num_states; j++) {
myfile << std::to_string(P[i][j]) << std::setprecision(1) << " ";
}
myfile << "\n\n\n\n\n\n\n\n\n\n";
}
}
#endif
// This checks if the diagonal of the covariance matrix is non-negative
// and that the matrix is symmetric
void Ekf::assertCovNiceness()
{
for (int row = 0; row < _k_num_states; row++) {
for (int column = 0; column < row; column++) {
assert(fabsf(P[row][column] - P[column][row]) < 0.00001f);
}
}
for (int i = 0; i < _k_num_states; i++) {
assert(P[i][i] > -0.000001f);
}
}
// This function forces the covariance matrix to be symmetric
void Ekf::makeSymmetrical()
{