mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-15 22:34:58 +00:00
Code Issues Releases Wiki Activity

FIX: doppler rate calculation error

Browse Source
This commit is contained in:
M.A. Gomez 2023-05-24 17:07:27 +02:00
parent adfa9db282
commit 948406acfe
3 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -162,7 +162,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; // kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt;
} }
obsv_calc(rho_pri, rhoDot_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x); obsv_calc(rho_pri, rhoDot_pri, rhoDot2_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x);
kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction
//************************************** //**************************************
@ -171,7 +171,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_H = arma::zeros(3 * new_data.sat_number, n_of_states); kf_H = arma::zeros(3 * new_data.sat_number, n_of_states);
kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt); kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt);
kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot_pri * 0, new_data.pr_m, new_data.doppler_hz, kf_x); kf_measurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, rhoDot2_pri, new_data.pr_m, new_data.doppler_hz, kf_x);
//************************************** //**************************************
// Kalman filter update step // Kalman filter update step
//************************************** //**************************************
@ -190,7 +190,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// Geometric Transformation // Geometric Transformation
//************************* //*************************
obsv_calc(rho_pri, rhoDot_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x); obsv_calc(rho_pri, rhoDot_pri, rhoDot2_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x);
kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt); kf_H_fill(kf_H, new_data.sat_number, a_x, a_y, a_z, kf_dt);
// Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x // Re-calculate error measurement vector with the most recent data available: kf_delta_y=kf_H*kf_delta_x
@ -315,7 +315,7 @@ void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt)
kf_F(9, 10) = kf_dt; kf_F(9, 10) = kf_dt;
} }
void Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x) void Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x)
{ {
for (int32_t i = 0; i < sat_number; i++) // neccesary quantities for (int32_t i = 0; i < sat_number; i++) // neccesary quantities
{ {
@ -333,7 +333,7 @@ void Vtl_Engine::obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colv
az(i) = -(sat_p(i, 2) - kf_x(2)) / d(i); az(i) = -(sat_p(i, 2) - kf_x(2)) / d(i);
// compute pseudorange rate estimation OUTPUT // compute pseudorange rate estimation OUTPUT
rhoDot_pri(i) = (sat_v(i, 0) - kf_x(3)) * a_x(i) + (sat_v(i, 1) - kf_x(4)) * a_y(i) + (sat_v(i, 2) - kf_x(5)) * a_z(i); rhoDot_pri(i) = (sat_v(i, 0) - kf_x(3)) * a_x(i) + (sat_v(i, 1) - kf_x(4)) * a_y(i) + (sat_v(i, 2) - kf_x(5)) * a_z(i);
// rhoDot_pri(i) = rhoDot_pri(i) + a_x(i)*xDot2_u*kf_dt+a_y(i)*yDot2_u*kf_dt+a_z(i)*zDot2_u*kf_dt; rhoDot2_pri(i) = (kf_x(6)) * a_x(i) + (kf_x(7)) * a_y(i) + (kf_x(8)) * a_z(i);
} }
} }
@ -375,7 +375,7 @@ void Vtl_Engine::model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::ma
if (t_disparo < .2) if (t_disparo < .2)
{ {
u_dir = {.90828, -.13984, -.388756}; u_dir = {.90828, -.13984, -.388756}; // launching attitude
} }
else else
{ {
@ -439,11 +439,11 @@ void Vtl_Engine::accelerometer(double &acc_x, double &acc_y, double &acc_z, arma
if (t_disparo < .2) if (t_disparo < .2)
{ {
u_dir = {.90828, -.13984, -.388756}; u_dir = {.90828, -.13984, -.388756}; // launching attitude
} }
else else
{ {
u_dir = u_vec / u; u_dir = u_vec / u;
} }
accelerometer = AccLkTable(t_disparo); accelerometer = AccLkTable(t_disparo);

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

@ -105,7 +105,7 @@ private:
void kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor void kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor
void kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor void kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor
void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_pri, arma::colvec &ax, arma::colvec &ay, arma::colvec &az, int sat_number, arma::mat sat_p, arma::mat sat_v, arma::mat kf_x); // Observables calculation
void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x);
void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter);
void accelerometer(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); void accelerometer(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter);

2
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
View File

@ -673,7 +673,7 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
// << " \n"; // << " \n";
} }
d_x_old_old(2) = tmp_x(2); // replace DOPPLER d_x_old_old(2) = tmp_x(2); // replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE
} }
else else
{ {