mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-15 22:34:58 +00:00
REFACTOR: vtl_engine
This commit is contained in:
M.A. Gomez
parent
50c7e151ab
commit
8056b54045
@ -33,31 +33,28 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
|
|||||||
//TODO: Implement main VTL loop here
|
//TODO: Implement main VTL loop here
|
||||||
using arma::as_scalar;
|
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 ######################################
|
// ################## 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)
|
// 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);
|
kf_R = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number);
|
||||||
double kf_dt = 0.05;
|
double kf_dt = delta_t_vtl; //0.05;
|
||||||
kf_Q = arma::eye(12, 12);
|
kf_Q = arma::eye(n_of_states, n_of_states);
|
||||||
|
|
||||||
kf_F = arma::eye(12, 12);
|
kf_F = arma::eye(n_of_states, n_of_states);
|
||||||
kf_F(0, 3) = kf_dt;
|
bool test = kf_F_fill(kf_F,kf_dt);
|
||||||
kf_F(1, 4) = kf_dt;
|
|
||||||
kf_F(2, 5) = kf_dt;
|
|
||||||
|
|
||||||
kf_F(3, 6) = kf_dt;
|
//kf_H = arma::zeros(2 * new_data.sat_number, n_of_states);
|
||||||
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_y = arma::zeros(2 * new_data.sat_number, 1);
|
kf_y = arma::zeros(2 * new_data.sat_number, 1);
|
||||||
kf_yerr = 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
|
kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number); // kf_P_y innovation covariance matrix
|
||||||
|
|
||||||
// ################## Kalman Tracking ######################################
|
// ################## Kalman Tracking ######################################
|
||||||
@ -68,36 +65,39 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
|
|||||||
|
|
||||||
if (counter < closure_point)
|
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(0) = new_data.rx_p(0);
|
||||||
kf_x(1) = new_data.rx_p(1);
|
kf_x(1) = new_data.rx_p(1);
|
||||||
kf_x(2) = new_data.rx_p(2);
|
kf_x(2) = new_data.rx_p(2);
|
||||||
kf_x(3) = new_data.rx_v(0);
|
kf_x(3) = new_data.rx_v(0);
|
||||||
kf_x(4) = new_data.rx_v(1);
|
kf_x(4) = new_data.rx_v(1);
|
||||||
kf_x(5) = new_data.rx_v(2);
|
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(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_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
|
else
|
||||||
{
|
{
|
||||||
// receiver solution from previous KF step
|
// receiver solution from previous KF step
|
||||||
kf_x(0) = new_data.kf_state[0];
|
// kf_x(0) = x_u;
|
||||||
kf_x(1) = new_data.kf_state[1];
|
// kf_x(1) = y_u;
|
||||||
kf_x(2) = new_data.kf_state[2];
|
// kf_x(2) = z_u;
|
||||||
kf_x(3) = new_data.kf_state[3];
|
// kf_x(3) = xDot_u;
|
||||||
kf_x(4) = new_data.kf_state[4];
|
// kf_x(4) = yDot_u;
|
||||||
kf_x(5) = new_data.kf_state[5];
|
// kf_x(5) = zDot_u;
|
||||||
kf_x(6) = new_data.kf_state[6];
|
// kf_x(6) = xDot2_u;
|
||||||
kf_x(7) = new_data.kf_state[7];
|
// kf_x(7) = yDot2_u;
|
||||||
kf_x(8) = new_data.kf_state[8];
|
// kf_x(8) = zDot2_u;
|
||||||
kf_x(9) = new_data.kf_state[9];
|
// kf_x(9) = cdeltat_u;
|
||||||
kf_x(10) = new_data.kf_state[10];
|
// kf_x(10) = cdeltatDot_u;
|
||||||
kf_x(11) = new_data.kf_state[11];
|
|
||||||
kf_P_x = new_data.kf_P;
|
//kf_P_x = new_data.kf_P;
|
||||||
}
|
}
|
||||||
|
|
||||||
// State error Covariance Matrix Q (PVT)
|
// 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(6, 6) = .1;
|
||||||
kf_Q(7, 7) = .1;
|
kf_Q(7, 7) = .1;
|
||||||
kf_Q(8, 8) = .1;
|
kf_Q(8, 8) = .1;
|
||||||
kf_Q(9, 9) = 100.0;
|
kf_Q(9, 9) = 4.0;
|
||||||
kf_Q(10, 10) = 40.0;
|
kf_Q(10, 10) = 100.0;
|
||||||
kf_Q(11, 11) = 1.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++)
|
||||||
{
|
{
|
||||||
// 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) = 50.0*50.0/new_data.sat_CN0_dB_hz(i); //TODO: fill with real values.
|
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) = 5.0*50.0/new_data.sat_CN0_dB_hz(i);
|
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){
|
// 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, 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;
|
||||||
}
|
// }
|
||||||
}
|
}
|
||||||
|
// arma::mat test = kf_P_x.diag();
|
||||||
|
// test.print("P diagonal");
|
||||||
|
// test = kf_Q.diag();
|
||||||
|
// test.print("Q diagonal");
|
||||||
// Kalman state prediction (time update)
|
// Kalman state prediction (time update)
|
||||||
//new_data.kf_state=kf_x;
|
//new_data.kf_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
|
|
||||||
//from error state variables to variables
|
//from error state variables to variables
|
||||||
// From state variables definition
|
// From state variables definition
|
||||||
// TODO: cast to type properly
|
// 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);
|
d = arma::zeros(new_data.sat_number, 1);
|
||||||
rho_pri = 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_x = arma::zeros(new_data.sat_number, 1);
|
||||||
a_y = 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);
|
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
|
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, 0) = a_x(i);
|
||||||
new_data.sat_LOS(i, 1) = a_y(i);
|
new_data.sat_LOS(i, 1) = a_y(i);
|
||||||
new_data.sat_LOS(i, 2) = a_z(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";
|
// cout<<new_data.sat_number<<"sat_number";
|
||||||
|
|
||||||
kf_H = arma::zeros(2 * new_data.sat_number, 12);
|
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);
|
||||||
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H assembling
|
// kf_H.print("kf_H diag:");
|
||||||
{
|
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
// Kalman estimation (measurement update)
|
// Kalman estimation (measurement update)
|
||||||
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector
|
test = kf_NLmeasurements(kf_yerr, new_data.sat_number, rho_pri, rhoDot_pri, new_data.pr_m, new_data.doppler_hz, kf_x);
|
||||||
{
|
|
||||||
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);
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
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
|
// Kalman filter update step
|
||||||
kf_S = kf_H * kf_P_x * kf_H.t() + kf_R; // innovation covariance matrix (S)
|
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_K = (kf_P_x * kf_H.t()) * arma::inv(kf_S); // Kalman gain
|
||||||
kf_xerr = kf_K * (kf_yerr); // Error state estimation
|
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
|
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 ######################################
|
// // ################## Geometric Transformation ######################################
|
||||||
|
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);
|
||||||
// // x_u=kf_x(0);
|
test = kf_H_fill(kf_H,new_data.sat_number,a_x, a_y, a_z, kf_dt);
|
||||||
// // 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;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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
|
||||||
kf_yerr = kf_H * kf_xerr;
|
kf_yerr = kf_H * kf_xerr;
|
||||||
// Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
|
// 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
|
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
|
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
|
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;
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
//TODO: Fill the tracking commands outputs
|
//TODO: Fill the tracking commands outputs
|
||||||
// Notice: keep the same satellite order as in the Vtl_Data matrices
|
// Notice: keep the same satellite order as in the Vtl_Data matrices
|
||||||
// sample code
|
// sample code
|
||||||
trk_cmd.carrier_phase_rads = 0; // difficult of calculation
|
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_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.code_phase_chips = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3;
|
||||||
trk_cmd.enable_carrier_nco_cmd = true;
|
trk_cmd.enable_carrier_nco_cmd = true;
|
||||||
trk_cmd.enable_code_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;
|
fstream dump_vtl_file;
|
||||||
dump_vtl_file.open("dump_vtl_file.csv", ios::out | ios::app);
|
dump_vtl_file.open("dump_vtl_file.csv", ios::out | ios::app);
|
||||||
dump_vtl_file.precision(15);
|
dump_vtl_file.precision(15);
|
||||||
@ -320,14 +223,29 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
|
|||||||
}
|
}
|
||||||
else
|
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"
|
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"
|
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;
|
<< "," << 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"
|
// 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;
|
// << "," << 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();
|
dump_vtl_file.close();
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
@ -348,3 +266,74 @@ void Vtl_Engine::configure(Vtl_Conf config_)
|
|||||||
config = config_;
|
config = config_;
|
||||||
//TODO: initialize internal variables
|
//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;
|
||||||
|
}
|
||||||
@ -52,19 +52,6 @@ private:
|
|||||||
Vtl_Conf config;
|
Vtl_Conf config;
|
||||||
//TODO: Internal VTL persistent variables here
|
//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
|
// Transformation variables
|
||||||
arma::colvec d;
|
arma::colvec d;
|
||||||
arma::colvec rho_pri;
|
arma::colvec rho_pri;
|
||||||
@ -78,7 +65,7 @@ private:
|
|||||||
|
|
||||||
// Kalman filter matrices
|
// Kalman filter matrices
|
||||||
arma::mat kf_P_x_ini; // initial state error covariance matrix
|
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_P_x_pre; // Predicted state error covariance matrix
|
||||||
arma::mat kf_S; // innovation covariance matrix
|
arma::mat kf_S; // innovation covariance matrix
|
||||||
|
|
||||||
@ -96,8 +83,12 @@ private:
|
|||||||
|
|
||||||
// Gaussian estimator
|
// Gaussian estimator
|
||||||
arma::mat kf_R_est; // measurement error covariance
|
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);
|
||||||
|
};
|
||||||
|
|
||||||
/** \} */
|
/** \} */
|
||||||
/** \} */
|
/** \} */
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user