#pragma once #include <iostream> #include <fstream> #include <Eigen/Dense> #include <string> #include <vector> /////////////////////////////////////////////////////////// // QRP_config File struct QRPConfigRawData { //////// Q //////// double sigma_accl_noise; // [m/s^2] (value derived from Noise Spectral Density in datasheet) double sigma_gyro_noise; // [rad/s] (value derived from Noise Spectral Density in datasheet) //////// R //////// Eigen::Vector3d sigma_pos; Eigen::Vector3d sigma_theta; Eigen::Vector3d sigma_vel; //////// P //////// double sigma_init_pos; double sigma_init_vel; double sigma_init_dtheta; }; // QRP_config_auto File struct QRPConfigAutoData { //////// Q //////// double sigma_accl_noise; // [m/s^2] (value derived from Noise Spectral Density in datasheet) double sigma_gyro_noise; // [rad/s] (value derived from Noise Spectral Density in datasheet) //////// R //////// Eigen::Vector4d sigma_pos_x; // r_min,r_max,a,k Eigen::Vector4d sigma_pos_y; Eigen::Vector4d sigma_pos_z; Eigen::Vector4d sigma_theta_roll; Eigen::Vector4d sigma_theta_pitch; Eigen::Vector4d sigma_theta_yaw; Eigen::Vector4d sigma_vel_x; Eigen::Vector4d sigma_vel_y; Eigen::Vector4d sigma_vel_z; //////// P //////// double sigma_init_pos; double sigma_init_vel; double sigma_init_dtheta; }; class QRPConfigFileReader { public: QRPConfigFileReader() {} ~QRPConfigFileReader() {} int loadQRPConfigFile(FILE *QRPConfigFile); int loadQRPConfigAutoFile(FILE *QRPConfigFile); std::vector<QRPConfigRawData> raw_data; std::vector<QRPConfigAutoData> auto_data; private: //bool parseFromString(std::string line, QRPConfigRawData &qrp_data); };