mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-08-06 05:43:47 +00:00
Code Issues Releases Wiki Activity

[MOD] debug over state added

Browse Source
This commit is contained in:
M.A.Gomez 2022-10-10 18:30:51 +02:00
parent 229c9115e4
commit 6fad047070
3 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/algorithms/PVT/libs/vtl_data.cc
View File

@ -42,6 +42,7 @@ void Vtl_Data::init_storage(int n_sats)
rx_dts = arma::mat(1, 2); rx_dts = arma::mat(1, 2);
rx_var = arma::vec(1); rx_var = arma::vec(1);
rx_pvt_var = arma::vec(8); rx_pvt_var = arma::vec(8);
kf_state = arma::vec(8);
epoch_tow_s = 0; epoch_tow_s = 0;
sample_counter = 0; sample_counter = 0;
} }
@ -54,8 +55,9 @@ void Vtl_Data::debug_print()
// sat_dts.print("VTL Sat clocks"); // sat_dts.print("VTL Sat clocks");
// sat_var.print("VTL Sat clock variances"); // sat_var.print("VTL Sat clock variances");
sat_health_flag.print("VTL Sat health"); sat_health_flag.print("VTL Sat health");
// kf_state.print("EKF STATE");
// pr_m.print("Satellite Code pseudoranges [m]"); pr_m.print("Satellite Code pseudoranges [m]");
// doppler_hz.print("satellite Carrier Dopplers [Hz]"); // doppler_hz.print("satellite Carrier Dopplers [Hz]");
// carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]");
} }

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

@ -50,6 +50,7 @@ public:
arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s] arma::mat rx_pvt_var; // Receiver position, velocity and time VARIANCE [m/s]
arma::mat rx_dts; // Receiver clock bias and drift [s,s/s] arma::mat rx_dts; // Receiver clock bias and drift [s,s/s]
arma::colvec rx_var; // Receiver position and clock error variance [m^2] arma::colvec rx_var; // Receiver position and clock error variance [m^2]
arma::colvec kf_state; // KF STATE
// time handling // time handling
double epoch_tow_s; // current observation RX time [s] double epoch_tow_s; // current observation RX time [s]
uint64_t sample_counter; // current sample counter associated with RX time [samples from start] uint64_t sample_counter; // current sample counter associated with RX time [samples from start]

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

@ -15,6 +15,9 @@
*/ */
#include "vtl_engine.h" #include "vtl_engine.h"
#include "iostream"
using namespace std;
Vtl_Engine::Vtl_Engine() Vtl_Engine::Vtl_Engine()
{ {
@ -30,7 +33,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
using arma::as_scalar; using arma::as_scalar;
// ################## Kalman filter initialization ###################################### // ################## Kalman filter initialization ######################################
// covariances (static) // covariances (static)
kf_P_x = arma::zeros(8, 8); //TODO: use a real value. kf_P_x = arma::eye(8, 8); //TODO: use a real value.
kf_x = arma::zeros(8, 1); kf_x = arma::zeros(8, 1);
kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number);
double kf_dt=1e-1; double kf_dt=1e-1;
@ -69,8 +72,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
} }
// // Kalman state prediction (time update) // // Kalman state prediction (time update)
cout << " KF RTKlib STATE" << kf_x;
kf_x = kf_F * kf_x; // state prediction kf_x = kf_F * kf_x; // state prediction
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
new_data.kf_state=kf_x;
cout << " KF priori STATE" << kf_x;
//from error state variables to variables //from error state variables to variables
//x_u=x_u0+kf_x_pri(0); //x_u=x_u0+kf_x_pri(0);
//y_u=y_u0+kf_x_pri(1); //y_u=y_u0+kf_x_pri(1);
@ -148,7 +154,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
//kf_delta_x = kf_K * kf_delta_y; // updated error state estimation //kf_delta_x = kf_K * kf_delta_y; // updated error state estimation
kf_x = kf_x + kf_K * (kf_y-kf_H*kf_x); // updated state estimation kf_x = kf_x + kf_K * (kf_y-kf_H*kf_x); // updated state estimation
kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix
cout << " KF posteriori STATE" << kf_x;
// // // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?) // // // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?)