mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-09-02 19:07:59 +00:00
Code Issues Releases Wiki Activity

Fallback to estimation in absence of IMU data

Browse Source
This commit is contained in:
Victor Castillo
2024-10-06 16:25:42 +02:00
parent 6e502aae33
commit 8636aa1788
3 changed files with 22 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@@ -1999,13 +1999,16 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
// | 0 1 0 |
// P_NED_ENU = | 1 0 0 |
// | 0 0 -1 |
//
// TODO:
// Plus temporary inversion of the North values due to IMU placement
// Plus temporary inversion of the Up values due to IMU placement
// I think, the IMU is mounted rotated 180 deg around the East axis
imu_data.data_available = true;
imu_data.vel(0) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[1 * sizeof(float)]));
imu_data.vel(1) =- static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[0 * sizeof(float)]));
imu_data.vel(1) = -static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[0 * sizeof(float)]));
imu_data.vel(2) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[2 * sizeof(float)]));
imu_data.acc(0) = static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[4 * sizeof(float)]));
imu_data.acc(1) = -static_cast<double>(*reinterpret_cast<float*>(&extra_data_bytes[3 * sizeof(float)]));

2
src/algorithms/PVT/libs/vtl_data.h
View File

@@ -71,6 +71,8 @@ public:
arma::colvec acc; // Receiver ENU acceleration from IMU [m/(s^2)]
arma::colvec ang_vel; // Receiver ENU angular velocity from IMU [rad/s]
arma::colvec ang_acc; // Receiver ENU angular velocity from IMU [rad/(s^2)]
bool data_available = false;
} imu_data;
void debug_print();

24
src/algorithms/PVT/libs/vtl_engine.cc
View File

@@ -133,12 +133,18 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_x(0) = new_data.rx_p(0);
kf_x(1) = new_data.rx_p(1);
kf_x(2) = new_data.rx_p(2);
kf_x(3) = new_data.rx_v(0);
kf_x(4) = new_data.rx_v(1);
kf_x(5) = new_data.rx_v(2);
kf_x(6) = 0;
kf_x(7) = 0;
kf_x(8) = 0;
kf_x(3) = new_data.imu_data.data_available ? new_data.imu_data.vel(0) : new_data.rx_v(0);
kf_x(4) = new_data.imu_data.data_available ? new_data.imu_data.vel(1) : new_data.rx_v(1);
kf_x(5) = new_data.imu_data.data_available ? new_data.imu_data.vel(2) : new_data.rx_v(2);
kf_x(6) = new_data.imu_data.data_available ? new_data.imu_data.acc(0) : 0.0;
kf_x(7) = new_data.imu_data.data_available ? new_data.imu_data.acc(1) : 0.0;
kf_x(8) = new_data.imu_data.data_available ? new_data.imu_data.acc(2) : 0.0;
// kf_x(3) = new_data.rx_v(0);
// kf_x(4) = new_data.rx_v(1);
// kf_x(5) = new_data.rx_v(2);
// kf_x(6) = 0;
// kf_x(7) = 0;
// kf_x(8) = 0;
kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S;
kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S;
@@ -150,9 +156,9 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// receiver solution from previous KF step
kf_x = kf_x;
// acceleration model
double acc_x = 0;
double acc_y = 0;
double acc_z = 0;
double acc_x = new_data.imu_data.data_available ? new_data.imu_data.acc(0) : 0.0;
double acc_y = new_data.imu_data.data_available ? new_data.imu_data.acc(1) : 0.0;
double acc_z = new_data.imu_data.data_available ? new_data.imu_data.acc(2) : 0.0;
kf_x(6) = acc_x;
kf_x(7) = acc_y;
kf_x(8) = acc_z;