You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
77 lines
1.2 KiB
77 lines
1.2 KiB
#include "ekf_logger.h" |
|
|
|
EkfLogger::EkfLogger(std::shared_ptr<Ekf> ekf): |
|
_ekf{ekf}, |
|
_ekf_wrapper(ekf) |
|
{ |
|
|
|
} |
|
|
|
EkfLogger::~EkfLogger() {} |
|
|
|
void EkfLogger::setFilePath(std::string file_path) |
|
{ |
|
_file_path = file_path; |
|
} |
|
|
|
void EkfLogger::writeStateToFile() |
|
{ |
|
if(!_file_opened) |
|
{ |
|
_file.open(_file_path); |
|
_file_opened = true; |
|
_file << "Timestamp"; |
|
if(_state_logging_enabled) |
|
{ |
|
for(int i = 0; i < 24; i++) |
|
{ |
|
_file << ",state[" << i << "]"; |
|
} |
|
} |
|
if(_variance_logging_enabled) |
|
{ |
|
for(int i = 0; i < 24; i++) |
|
{ |
|
_file << ",variance[" << i << "]"; |
|
} |
|
} |
|
_file << std::endl; |
|
} |
|
|
|
if (_file) |
|
{ |
|
writeState(); |
|
} |
|
else |
|
{ |
|
std::cerr << "Can not write to output file" << std::endl; |
|
std::exit(-1); |
|
} |
|
|
|
} |
|
|
|
void EkfLogger::writeState() |
|
{ |
|
if(_state_logging_enabled) |
|
{ |
|
uint64_t time = _ekf->get_imu_sample_delayed().time_us; |
|
_file << time; |
|
if(_state_logging_enabled) |
|
{ |
|
matrix::Vector<float, 24> state = _ekf->getStateAtFusionHorizonAsVector(); |
|
for(int i = 0; i < 24; i++) |
|
{ |
|
_file << "," << state(i); |
|
} |
|
} |
|
if(_variance_logging_enabled) |
|
{ |
|
matrix::Vector<float, 24> variance = _ekf->covariances_diagonal(); |
|
for(int i = 0; i < 24; i++) |
|
{ |
|
_file << "," << variance(i); |
|
} |
|
} |
|
_file << std::endl; |
|
} |
|
}
|
|
|