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

REFACTOR: vtl_engine

Browse Source
This commit is contained in:
M.A. Gomez 2023-02-12 12:58:55 +01:00
parent 50c7e151ab
commit 8056b54045
2 changed files with 166 additions and 186 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -33,31 +33,28 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
//TODO: Implement main VTL loop here
using arma::as_scalar;
static uint64_t refSampleCounter = new_data.sample_counter;
double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0;
refSampleCounter = new_data.sample_counter;
// ################## Kalman filter initialization ######################################
//State variables
int n_of_states=11;
static arma::mat kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0; //TODO: use a real value.;
static arma::mat kf_x = arma::zeros(n_of_states, 1);
arma::mat kf_dx = arma::zeros(n_of_states, 1);
// covariances (static)
kf_P_x = arma::eye(12, 12) * 1.0; //TODO: use a real value.
kf_x = arma::zeros(12, 1);
kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number);
double kf_dt = 0.05;
kf_Q = arma::eye(12, 12);
double kf_dt = delta_t_vtl; //0.05;
kf_Q = arma::eye(n_of_states, n_of_states);
kf_F = arma::eye(12, 12);
kf_F(0, 3) = kf_dt;
kf_F(1, 4) = kf_dt;
kf_F(2, 5) = kf_dt;
kf_F = arma::eye(n_of_states, n_of_states);
bool test = kf_F_fill(kf_F,kf_dt);
kf_F(3, 6) = kf_dt;
kf_F(4, 7) = kf_dt;
kf_F(5, 8) = kf_dt;
kf_F(9, 10) = kf_dt;
kf_F(10, 11) = kf_dt;
kf_H = arma::zeros(2 * new_data.sat_number, 12);
//kf_H = arma::zeros(2 * new_data.sat_number, n_of_states);
kf_y = arma::zeros(2 * new_data.sat_number, 1);
kf_yerr = arma::zeros(2 * new_data.sat_number, 1);
kf_xerr = arma::zeros(12, 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
// ################## Kalman Tracking ######################################
@ -68,36 +65,39 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
if (counter < closure_point)
{
// receiver solution from rtklib_solver
// // receiver solution from rtklib_solver
kf_dx=kf_x;
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(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(11) = 1.0;
kf_x = kf_F * kf_x; // state prediction
kf_dx=kf_x-kf_dx;
kf_dx = kf_F * kf_dx; // state prediction
}
else
{
// receiver solution from previous KF step
kf_x(0) = new_data.kf_state[0];
kf_x(1) = new_data.kf_state[1];
kf_x(2) = new_data.kf_state[2];
kf_x(3) = new_data.kf_state[3];
kf_x(4) = new_data.kf_state[4];
kf_x(5) = new_data.kf_state[5];
kf_x(6) = new_data.kf_state[6];
kf_x(7) = new_data.kf_state[7];
kf_x(8) = new_data.kf_state[8];
kf_x(9) = new_data.kf_state[9];
kf_x(10) = new_data.kf_state[10];
kf_x(11) = new_data.kf_state[11];
kf_P_x = new_data.kf_P;
// kf_x(0) = x_u;
// kf_x(1) = y_u;
// kf_x(2) = z_u;
// kf_x(3) = xDot_u;
// kf_x(4) = yDot_u;
// kf_x(5) = zDot_u;
// kf_x(6) = xDot2_u;
// kf_x(7) = yDot2_u;
// kf_x(8) = zDot2_u;
// kf_x(9) = cdeltat_u;
// kf_x(10) = cdeltatDot_u;
//kf_P_x = new_data.kf_P;
}
// State error Covariance Matrix Q (PVT)
@ -111,41 +111,31 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
kf_Q(6, 6) = .1;
kf_Q(7, 7) = .1;
kf_Q(8, 8) = .1;
kf_Q(9, 9) = 100.0;
kf_Q(10, 10) = 40.0;
kf_Q(11, 11) = 1.0;
kf_Q(9, 9) = 4.0;
kf_Q(10, 10) = 100.0;
// Measurement error Covariance Matrix R assembling
for (int32_t i = 0; i < new_data.sat_number; i++)
{
// It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error)
kf_R(i, i) = 50.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) = 5.0*50.0/new_data.sat_CN0_dB_hz(i);
kf_R(i, i) = 20.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) = 1.0;//*50.0/new_data.sat_CN0_dB_hz(i);
if(50.0*50.0/new_data.sat_CN0_dB_hz(i)>70||5.0*50.0/new_data.sat_CN0_dB_hz(i)>7){
kf_R(i, i) = 10e4;
kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4;
// if(50.0*50.0/new_data.sat_CN0_dB_hz(i)>70||5.0*50.0/new_data.sat_CN0_dB_hz(i)>7){
// kf_R(i, i) = 10e4;
// kf_R(i + new_data.sat_number, i + new_data.sat_number) = 10e4;
// }
}
}
// arma::mat test = kf_P_x.diag();
// test.print("P diagonal");
// test = kf_Q.diag();
// test.print("Q diagonal");
// Kalman state prediction (time update)
//new_data.kf_state=kf_x;
//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
//from error state variables to variables
// From state variables definition
// TODO: cast to type properly
x_u = kf_x(0);
y_u = kf_x(1);
z_u = kf_x(2);
xDot_u = kf_x(3);
yDot_u = kf_x(4);
zDot_u = kf_x(5);
xDot2_u = kf_x(6);
yDot2_u = kf_x(7);
zDot2_u = kf_x(8);
cdeltat_u = kf_x(9);
cdeltatDot_u = kf_x(10);
d = arma::zeros(new_data.sat_number, 1);
rho_pri = arma::zeros(new_data.sat_number, 1);
@ -158,119 +148,41 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
a_x = arma::zeros(new_data.sat_number, 1);
a_y = arma::zeros(new_data.sat_number, 1);
a_z = arma::zeros(new_data.sat_number, 1);
test = 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);
for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities
{
//d(i) is the distance sat(i) to receiver
d(i) = (new_data.sat_p(i, 0) - x_u) * (new_data.sat_p(i, 0) - x_u);
d(i) = d(i) + (new_data.sat_p(i, 1) - y_u) * (new_data.sat_p(i, 1) - y_u);
d(i) = d(i) + (new_data.sat_p(i, 2) - z_u) * (new_data.sat_p(i, 2) - z_u);
d(i) = sqrt(d(i));
//compute pseudorange estimation
rho_pri(i) = d(i) + cdeltat_u;
//compute LOS sat-receiver vector components
a_x(i) = -(new_data.sat_p(i, 0) - x_u) / d(i);
a_y(i) = -(new_data.sat_p(i, 1) - y_u) / d(i);
a_z(i) = -(new_data.sat_p(i, 2) - z_u) / d(i);
new_data.sat_LOS(i, 0) = a_x(i);
new_data.sat_LOS(i, 1) = a_y(i);
new_data.sat_LOS(i, 2) = a_z(i);
//compute pseudorange rate estimation
rhoDot_pri(i) = (new_data.sat_v(i, 0) - xDot_u) * a_x(i) + (new_data.sat_v(i, 1) - yDot_u) * a_y(i) + (new_data.sat_v(i, 2) - zDot_u) * 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;;
}
// cout<<new_data.sat_number<<"sat_number";
kf_H = arma::zeros(2 * new_data.sat_number, 12);
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H assembling
{
// It has 12 columns (12 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(i, 0) = a_x(i);
kf_H(i, 1) = a_y(i);
kf_H(i, 2) = a_z(i);
kf_H(i, 9) = 1.0;
kf_H(i + new_data.sat_number, 3) = a_x(i);
kf_H(i + new_data.sat_number, 4) = a_y(i);
kf_H(i + new_data.sat_number, 5) = a_z(i);
kf_H(i + new_data.sat_number, 6) = a_x(i)*kf_dt;
kf_H(i + new_data.sat_number, 7) = a_y(i)*kf_dt;
kf_H(i + new_data.sat_number, 8) = a_z(i)*kf_dt;
kf_H(i + new_data.sat_number, 10) = 1.0;
}
kf_H = arma::zeros(2 * new_data.sat_number, n_of_states);
test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt);
// kf_H.print("kf_H diag:");
// Kalman estimation (measurement update)
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector
{
kf_yerr(i) = rho_pri(i) - new_data.pr_m(i);
if (counter < closure_point)
{
kf_yerr(i + new_data.sat_number) = (new_data.doppler_hz(i) * Lambda_GPS_L1) - rhoDot_pri(i);
}else{
kf_yerr(i + new_data.sat_number) = (new_data.doppler_hz(i) * Lambda_GPS_L1+cdeltatDot_u) - rhoDot_pri(i);
}
}
test = kf_NLmeasurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x);
kf_P_x = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance prediction
// kf_P_x.print("a priori P: ");
// Kalman filter update step
kf_S = kf_H * kf_P_x * kf_H.t() + kf_R; // innovation covariance matrix (S)
kf_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain
kf_xerr = kf_K * (kf_yerr); // Error state estimation
kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error)
kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix
new_data.kf_P = kf_P_x;
kf_x = kf_x;// - kf_xerr; // updated state estimation (a priori + error)
if(abs(delta_t_vtl)>.5){
kf_xerr.print("kf_xERR: ");
cout<<"kf_dt: "<<delta_t_vtl<<endl;
kf_x.print("kf_x:");
}
// kf_x.print("state CORRECTED:");
// // ################## Geometric Transformation ######################################
// // x_u=kf_x(0);
// // y_u=kf_x(1);
// // z_u=kf_x(2);
// // xDot_u=kf_x(3);
// // yDot_u=kf_x(4);
// // zDot_u=kf_x(5);
// // cdeltat_u=kf_x(6)*SPEED_OF_LIGHT_M_S;
// // cdeltatDot_u=kf_x(7)*SPEED_OF_LIGHT_M_S;
for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities
{
//d(i) is the distance sat(i) to receiver
d(i) = (new_data.sat_p(i, 0) - kf_x(0)) * (new_data.sat_p(i, 0) - kf_x(0));
d(i) = d(i) + (new_data.sat_p(i, 1) - kf_x(1)) * (new_data.sat_p(i, 1) - kf_x(1));
d(i) = d(i) + (new_data.sat_p(i, 2) - kf_x(2)) * (new_data.sat_p(i, 2) - kf_x(2));
d(i) = sqrt(d(i));
//compute pseudorange estimation
rho_pri(i) = d(i) + kf_x(6);
//compute LOS sat-receiver vector components
a_x(i) = -(new_data.sat_p(i, 0) - kf_x(0)) / d(i);
a_y(i) = -(new_data.sat_p(i, 1) - kf_x(1)) / d(i);
a_z(i) = -(new_data.sat_p(i, 2) - kf_x(2)) / d(i);
//compute pseudorange rate estimation
rhoDot_pri(i) = (new_data.sat_v(i, 0) - kf_x(3)) * a_x(i) + (new_data.sat_v(i, 1) - kf_x(4)) * a_y(i) + (new_data.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;
}
kf_H = arma::zeros(2 * new_data.sat_number, 12);
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H assembling
{
// It has 12 columns (12 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(i, 0) = a_x(i);
kf_H(i, 1) = a_y(i);
kf_H(i, 2) = a_z(i);
kf_H(i, 9) = 1.0;
kf_H(i + new_data.sat_number, 3) = a_x(i);
kf_H(i + new_data.sat_number, 4) = a_y(i);
kf_H(i + new_data.sat_number, 5) = a_z(i);
kf_H(i + new_data.sat_number, 6) = a_x(i)*kf_dt;
kf_H(i + new_data.sat_number, 7) = a_y(i)*kf_dt;
kf_H(i + new_data.sat_number, 8) = a_z(i)*kf_dt;
kf_H(i + new_data.sat_number, 10) = 1.0;
}
test = 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);
test = 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
kf_yerr = kf_H * kf_xerr;
// Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
@ -280,21 +192,14 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
for (int32_t channel = 0; channel < new_data.sat_number; channel++) // Measurement vector
{
rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered
rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1 + kf_x(7)) - kf_yerr(channel + new_data.sat_number); // now filtered
if(counter < closure_point)
{
doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)) / Lambda_GPS_L1;
}else{
doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)) / Lambda_GPS_L1;
}
rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1) - kf_yerr(channel + new_data.sat_number); // now filtered
doppler_hz_filt(channel) = (rhoDot_pri_filt(channel)) / Lambda_GPS_L1;
//TODO: Fill the tracking commands outputs
// Notice: keep the same satellite order as in the Vtl_Data matrices
// sample code
trk_cmd.carrier_phase_rads = 0; // difficult of calculation
trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction
trk_cmd.carrier_freq_rate_hz_s = 0;
trk_cmd.carrier_freq_rate_hz_s = (a_x(channel)*kf_x(6)+a_y(channel)*kf_x(7)+a_z(channel)*kf_x(8));
trk_cmd.code_phase_chips = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3;
trk_cmd.enable_carrier_nco_cmd = true;
trk_cmd.enable_code_nco_cmd = true;
@ -309,8 +214,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
// }
}
new_data.kf_state = kf_x; //updated state estimation
fstream dump_vtl_file;
dump_vtl_file.open("dump_vtl_file.csv", ios::out | ios::app);
dump_vtl_file.precision(15);
@ -320,14 +223,29 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
}
else
{
dump_vtl_file << "kf_xerr"
<< "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) << "," << kf_xerr(6) << "," << kf_xerr(7) << endl;
if(new_data.sat_number>5){
// dump_vtl_file << "pr_m"
// << "," << new_data.pr_m(0) << "," << new_data.pr_m(1) << "," << new_data.pr_m(2)
// << "," << new_data.pr_m(3) << "," << new_data.pr_m(4)<<"," << new_data.pr_m(5) << endl;
// dump_vtl_file << "prDot_m"
// << "," << new_data.doppler_hz(0)<< "," << new_data.doppler_hz(1)<< "," << new_data.doppler_hz(2)
// << "," << new_data.doppler_hz(3) << "," << new_data.doppler_hz(4)<< "," << new_data.doppler_hz(5)<< endl;
// dump_vtl_file << "sat_position"
// << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << "," << kf_xerr(5)
// << "," << kf_xerr(6) << "," << kf_xerr(7) << endl;
// dump_vtl_file << "sat_velocity"
// << "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5)
// << "," << kf_xerr(6) << "," << kf_xerr(7) << endl;
}
dump_vtl_file << "kf_state"
<< "," << new_data.kf_state(0) << "," << new_data.kf_state(1) << "," << new_data.kf_state(2) << "," << new_data.kf_state(3) << "," << new_data.kf_state(4) << "," << new_data.kf_state(5) << "," << new_data.kf_state(6) << "," << new_data.kf_state(7)<< new_data.kf_state(8) << endl;
<< "," << kf_x(0) << "," << kf_x(1) << "," << kf_x(2) << "," << kf_x(3) << "," << kf_x(4) << "," << kf_x(5) << "," << kf_x(6) << "," << kf_x(7)<< "," << kf_x(8) <<"," << kf_x(9) <<"," << kf_x(10)<< endl;
dump_vtl_file << "kf_xerr"
<< "," << kf_xerr(0) << "," << kf_xerr(1) << "," << kf_xerr(2) << "," << kf_xerr(3) << "," << kf_xerr(4) << "," << kf_xerr(5) << "," << kf_xerr(6) << "," << kf_xerr(7)<< "," << kf_xerr(8) <<"," << kf_xerr(9) <<"," << kf_xerr(10)<< endl;
dump_vtl_file << "rtklib_state"
<< "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << endl;
dump_vtl_file << "filt_dopp_sat"
<< "," << doppler_hz_filt(0) << "," << doppler_hz_filt(1) << "," << doppler_hz_filt(2) << "," << doppler_hz_filt(3) << "," << doppler_hz_filt(4) << endl;
<< "," << new_data.rx_p(0) << "," << new_data.rx_p(1) << "," << new_data.rx_p(2) << "," << new_data.rx_v(0) << "," << new_data.rx_v(1) << "," << new_data.rx_v(2) << "," << new_data.rx_dts(0) << "," << new_data.rx_dts(1) << "," << delta_t_vtl << endl;
// dump_vtl_file << "filt_dopp_sat"
// << "," << doppler_hz_filt(0) << "," << doppler_hz_filt(1) << "," << doppler_hz_filt(2) << "," << doppler_hz_filt(3) << "," << doppler_hz_filt(4) <<endl;
dump_vtl_file.close();
}
return true;
@ -348,3 +266,74 @@ void Vtl_Engine::configure(Vtl_Conf config_)
config = config_;
//TODO: initialize internal variables
}
bool Vtl_Engine::kf_H_fill(arma::mat &kf_H,int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt)
{
for (int32_t i = 0; i < sat_number; i++) // Measurement matrix H assembling
{
// It has n_of_states columns (n_of_states states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
kf_H(i, 0) = ax(i);
kf_H(i, 1) = ay(i);
kf_H(i, 2) = az(i);
kf_H(i, 9) = 1.0;
kf_H(i + sat_number, 3) = ax(i);
kf_H(i + sat_number, 4) = ay(i);
kf_H(i + sat_number, 5) = az(i);
kf_H(i + sat_number, 6) = ax(i)*kf_dt;
kf_H(i + sat_number, 7) = ay(i)*kf_dt;
kf_H(i + sat_number, 8) = az(i)*kf_dt;
kf_H(i + sat_number, 10) = 1.0;
}
return -1;
}
bool Vtl_Engine::kf_F_fill(arma::mat &kf_F,double kf_dt)
{
kf_F(0, 3) = kf_dt; kf_F(0, 6) = kf_dt*kf_dt/2;
kf_F(1, 4) = kf_dt; kf_F(1, 7) = kf_dt*kf_dt/2;
kf_F(2, 5) = kf_dt; kf_F(2, 8) = kf_dt*kf_dt/2;
kf_F(3, 6) = kf_dt;
kf_F(4, 7) = kf_dt;
kf_F(5, 8) = kf_dt;
kf_F(9, 10) = kf_dt;
return -1;
}
bool 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)
{
for (int32_t i = 0; i < sat_number; i++) //neccesary quantities
{
//d(i) is the distance sat(i) to receiver
d(i) = (sat_p(i, 0) - kf_x(0)) * (sat_p(i, 0) - kf_x(0));
d(i) = d(i) + (sat_p(i, 1) - kf_x(1)) * (sat_p(i, 1) - kf_x(1));
d(i) = d(i) + (sat_p(i, 2) - kf_x(2)) * (sat_p(i, 2) - kf_x(2));
d(i) = sqrt(d(i));
//compute pseudorange estimation
rho_pri(i) = d(i) + kf_x(9);
//compute LOS sat-receiver vector componentsx
ax(i) = -(sat_p(i, 0) - kf_x(0)) / d(i);
ay(i) = -(sat_p(i, 1) - kf_x(1)) / d(i);
az(i) = -(sat_p(i, 2) - kf_x(2)) / d(i);
//compute pseudorange rate estimation
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;
}
return -1;
}
bool Vtl_Engine::kf_NLmeasurements(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)
{
for (int32_t i = 0; i < sat_number; i++) // Measurement vector
{
kf_yerr(i) = rho_pri(i) - pr_m(i);
kf_yerr(i + sat_number) = (doppler_hz(i) * Lambda_GPS_L1+kf_x(10)) - rhoDot_pri(i);
}
return -1;
}

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

@ -52,19 +52,6 @@ private:
Vtl_Conf config;
//TODO: Internal VTL persistent variables here
//State variables
double x_u;
double y_u;
double z_u;
double xDot_u;
double yDot_u;
double zDot_u;
double xDot2_u;
double yDot2_u;
double zDot2_u;
double cdeltat_u;
double cdeltatDot_u;
// Transformation variables
arma::colvec d;
arma::colvec rho_pri;
@ -78,7 +65,7 @@ private:
// Kalman filter matrices
arma::mat kf_P_x_ini; // initial state error covariance matrix
arma::mat kf_P_x; // state error covariance matrix
// arma::mat kf_P_x; // state error covariance matrix
arma::mat kf_P_x_pre; // Predicted state error covariance matrix
arma::mat kf_S; // innovation covariance matrix
@ -96,8 +83,12 @@ private:
// Gaussian estimator
arma::mat kf_R_est; // measurement error covariance
};
bool 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
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 kf_NLmeasurements(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);
};
/** \} */
/** \} */