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

ADD: vtl variables and Q estimation.

Browse Source
This commit is contained in:
M.A.Gomez 2022-10-10 12:00:33 +02:00
parent 6f404626ff
commit c34782762c
2 changed files with 11 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.h
View File

@ -38,6 +38,7 @@ public:
arma::mat sat_dts; // Satellite clock bias and drift [s,s/s] arma::mat sat_dts; // Satellite clock bias and drift [s,s/s]
arma::colvec sat_var; // sat position and clock error variance [m^2] arma::colvec sat_var; // sat position and clock error variance [m^2]
arma::colvec sat_health_flag; // sat health flag (0 is ok) arma::colvec sat_health_flag; // sat health flag (0 is ok)
arma::colvec sat_CN0_dB_hz; // sat CN0 in dB-Hz
int sat_number; // on-view sat number int sat_number; // on-view sat number
arma::colvec pr_m; // Satellite Code pseudoranges [m] arma::colvec pr_m; // Satellite Code pseudoranges [m]
@ -46,8 +47,9 @@ public:
arma::mat rx_p; // Receiver ENU Position [m] arma::mat rx_p; // Receiver ENU Position [m]
arma::mat rx_v; // Receiver Velocity [m/s] arma::mat rx_v; // Receiver Velocity [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]
// 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

@ -64,6 +64,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_x(6)=new_data.rx_dts(0); kf_x(6)=new_data.rx_dts(0);
kf_x(7)=new_data.rx_dts(1); kf_x(7)=new_data.rx_dts(1);
for (int32_t i = 0; i < 8; i++) // State error Covariance Matrix Q (PVT)
{
// It is diagonal 8x8 matrix
kf_Q(i, i) = new_data.rx_pvt_var(i); //careful, values for V and T could not be adecuate.
}
// // Kalman state prediction (time update) // // Kalman state prediction (time update)
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
@ -128,7 +134,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement error Covariance Matrix R assembling
{ {
// It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error) // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error)
kf_R(i, i) = 1.0; //TODO: use a real value. kf_R(i, i) = 1.0; //TODO: use a valid value.
kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0; kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0;
} }
@ -143,7 +149,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_K * (kf_y-dot(kf_H,kf_x)); // updated error state estimation kf_x = kf_x + kf_K * (kf_y-dot(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
// // 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?)