mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-16 06:44:57 +00:00
Code Issues Releases Wiki Activity

ADD: model3DoF prototype

Browse Source
This commit is contained in:
M.A. Gomez 2023-02-15 16:33:02 +01:00
parent 1991ab4aa6
commit 7f93138c96
2 changed files with 29 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,19 +44,19 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
static arma::mat kf_dx = arma::zeros(n_of_states, 1); static arma::mat kf_dx = arma::zeros(n_of_states, 1);
// covariances (static) // covariances (static)
kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); kf_R = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number);
double kf_dt = delta_t_vtl; //0.05; double kf_dt = delta_t_vtl; //0.05;
kf_Q = arma::eye(n_of_states, n_of_states); kf_Q = arma::eye(n_of_states, n_of_states);
kf_F = arma::eye(n_of_states, n_of_states); kf_F = arma::eye(n_of_states, n_of_states);
bool test = kf_F_fill(kf_F,kf_dt); bool test = kf_F_fill(kf_F,kf_dt);
//kf_H = arma::zeros(2 * new_data.sat_number, n_of_states); //kf_H = arma::zeros(3 * new_data.sat_number, n_of_states);
kf_y = arma::zeros(2 * new_data.sat_number, 1); kf_y = arma::zeros(3 * new_data.sat_number, 1);
kf_yerr = arma::zeros(2 * new_data.sat_number, 1); kf_yerr = arma::zeros(3 * new_data.sat_number, 1);
kf_xerr = arma::zeros(n_of_states, 1); kf_xerr = arma::zeros(n_of_states, 1);
kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix
kf_K = arma::zeros(n_of_states, 2 * new_data.sat_number); ; kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number); ;
// ################## Kalman Tracking ###################################### // ################## Kalman Tracking ######################################
static uint32_t counter = 0; //counter static uint32_t counter = 0; //counter
counter = counter + 1; //uint64_t counter = counter + 1; //uint64_t
@ -79,15 +79,22 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
kf_x(9) = new_data.rx_dts(0) * SPEED_OF_LIGHT_M_S; 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; kf_x(10) = new_data.rx_dts(1) * SPEED_OF_LIGHT_M_S;
kf_dx=kf_x-kf_dx; kf_dx = kf_x-kf_dx;
kf_dx = kf_F * kf_dx; // state prediction kf_dx = kf_F * kf_dx; // state prediction
} }
else else
{ {
// receiver solution from previous KF step // receiver solution from previous KF step
kf_x(6) = (kf_x(6)-kf_dx(6))/kf_dt; double acc_x = 0;
kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; double acc_y = 0;
kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; double acc_z = 0;
test = model3DoF(acc_x,acc_x,acc_x,kf_x,kf_dt);
kf_x(6) = acc_x;
kf_x(7) = acc_y;
kf_x(8) = acc_z;
// kf_x(6) = (kf_x(6)-kf_dx(6))/kf_dt;
// kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt;
// kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt;
} }
// State error Covariance Matrix Q (PVT) // State error Covariance Matrix Q (PVT)
@ -98,11 +105,11 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
kf_Q(3, 3) = 8.0; kf_Q(3, 3) = 8.0;
kf_Q(4, 4) = 8.0; kf_Q(4, 4) = 8.0;
kf_Q(5, 5) = 8.0; kf_Q(5, 5) = 8.0;
kf_Q(6, 6) = 10; kf_Q(6, 6) = .10;
kf_Q(7, 7) = 10; kf_Q(7, 7) = .10;
kf_Q(8, 8) = 10; kf_Q(8, 8) = .10;
kf_Q(9, 9) = 4.0; kf_Q(9, 9) = 4.0;
kf_Q(10, 10) = 100.0; kf_Q(10, 10) = 10.0;
// Measurement error Covariance Matrix R assembling // Measurement error Covariance Matrix R assembling
for (int32_t i = 0; i < new_data.sat_number; i++) for (int32_t i = 0; i < new_data.sat_number; i++)
@ -110,10 +117,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
// 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) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values. kf_R(i, i) = 80.0;//*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values.
kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i); kf_R(i + new_data.sat_number, i + new_data.sat_number) = 20.0;//*50.0/new_data.sat_CN0_dB_hz(i);
kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 100.0;//*50.0/new_data.sat_CN0_dB_hz(i);
if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){ if(80.0*50.0/new_data.sat_CN0_dB_hz(i)>90||20.0*50.0/new_data.sat_CN0_dB_hz(i)>25){
kf_R(i, i) = 10e4; kf_R(i, i) = 10e4;
kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4; kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4;
kf_R(i + 2*new_data.sat_number, i + 2*new_data.sat_number) = 10e4;
cout<<"channel: "<<i<<"discarded"<<endl; cout<<"channel: "<<i<<"discarded"<<endl;
} }
} }
@ -321,3 +330,8 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r
} }
return -1; return -1;
} }
bool Vtl_Engine::model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double kf_dt)
{
return -1;
}

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

@ -88,6 +88,7 @@ private:
bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor
bool 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 bool 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
bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); bool kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x);
bool model3DoF(double acc_x,double acc_y,double acc_z,arma::mat kf_x,double kf_dt);
}; };
/** \} */ /** \} */