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

FIX: VTL-KF

Browse Source
This commit is contained in:
M.A.Gomez 2022-10-13 18:13:35 +02:00
parent 6fad047070
commit 86f2e6b5d3
4 changed files with 57 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -42,7 +42,9 @@
#include <exception>
#include <utility>
#include <vector>
#include "iostream"
using namespace std;
Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
const std::string &dump_filename,
@ -1138,7 +1140,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler
//receiver clock offset and receiver clock drift
new_vtl_data.rx_dts(0)=rx_position_and_time[3];
new_vtl_data.rx_dts(1)=pvt_sol.dtr[5];
new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]/1e6; // [ppm] to [s]
//Call the VTL engine loop: miguel: Should we wait until valid PVT solution?
vtl_engine.vtl_loop(new_vtl_data);

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

@ -57,7 +57,7 @@ void Vtl_Data::debug_print()
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]");
// carrier_phase_rads.print("satellite accumulated carrier phases [rads]");
}

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

@ -52,6 +52,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_H = arma::zeros(8, 2*new_data.sat_number);
kf_x = arma::zeros(8, 1);
kf_y = arma::zeros(2*new_data.sat_number, 1);
kf_yerr = arma::zeros(2*new_data.sat_number, 1);
kf_xerr = arma::zeros(8, 1);
kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix
// ################## Kalman Tracking ######################################
@ -73,19 +75,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// // Kalman state prediction (time update)
cout << " KF RTKlib STATE" << kf_x;
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
new_data.kf_state=kf_x;
cout << " KF priori STATE" << kf_x;
// cout << " KF priori STATE diference" << kf_x-new_data.kf_state;
//from error state variables to variables
//x_u=x_u0+kf_x_pri(0);
//y_u=y_u0+kf_x_pri(1);
//z_u=z_u0+kf_x_pri(2);
//xDot_u=xDot_u0+kf_x_pri(3);
//yDot_u=yDot_u0+kf_x_pri(4);
//zDot_u=zDot_u0+kf_x_pri(5);
//cdeltat_u=cdeltat_u0+kf_x_pri(6);
//cdeltatDot_u=cdeltatDot_u+kf_x_pri(7);
kf_xerr=kf_x-new_data.kf_state;
// From state variables definition
x_u=kf_x(0);
y_u=kf_x(1);
@ -93,8 +88,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
xDot_u=kf_x(3);
yDot_u=kf_x(4);
zDot_u=kf_x(5);
cdeltat_u=kf_x(6);
cdeltatDot_u=kf_x(7);
cdeltat_u=kf_x(6)*SPEED_OF_LIGHT_M_S;
cdeltatDot_u=kf_x(7)*SPEED_OF_LIGHT_M_S;
d = arma::zeros(new_data.sat_number, 1);
rho_pri = arma::zeros(new_data.sat_number, 1);
@ -102,16 +97,15 @@ 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);
for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities
{
d(i)=(sqrt((new_data.sat_p(i, 0)-x_u)*(new_data.sat_p(i, 0)-x_u)+(new_data.sat_p(i, 1)-y_u)*(new_data.sat_p(i, 1)-y_u)+(new_data.sat_p(i, 2)-z_u)*(new_data.sat_p(i, 2)-z_u)));
d(i)=sqrt((new_data.sat_p(i, 0)-x_u)*(new_data.sat_p(i, 0)-x_u)+(new_data.sat_p(i, 1)-y_u)*(new_data.sat_p(i, 1)-y_u)+(new_data.sat_p(i, 2)-z_u)*(new_data.sat_p(i, 2)-z_u));
//compute pseudorange estimation
rho_pri(i)=d(i)+cdeltat_u;
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);;
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);
//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)+cdeltatDot_u;
}
@ -124,16 +118,17 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_H(i, 0) = a_x(i); kf_H(i, 1) = a_y(i); kf_H(i, 2) = a_z(i); kf_H(i, 6) = 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, 7) = 1.0;
}
// Kalman estimation (measurement update)
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector
{
//kf_y(i) = delta_rho(i); // i-Satellite
kf_y(i)=new_data.pr_m(i);
kf_yerr(i)=kf_y(i)-rho_pri(i);
//kf_y(i+new_data.sat_number) = delta_rhoDot(i); // i-Satellite
kf_y(i+new_data.sat_number)=new_data.doppler_hz(i);
kf_y(i+new_data.sat_number)=-new_data.doppler_hz(i)/0.1902937+cdeltatDot_u;
kf_yerr(i+new_data.sat_number)=kf_y(i+new_data.sat_number)-rhoDot_pri(i);
}
//cout << " KF yerr" << kf_yerr;
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)
@ -152,45 +147,49 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
//}
//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_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix
//cout << " KF measures ERROR" << kf_yerr;
//cout << " KF measure ERROR stimation" << kf_H*kf_x;
cout << " KF innovation" << kf_yerr-kf_H*kf_xerr;
kf_x = kf_x + kf_K * (kf_yerr-kf_H*kf_xerr); // 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
cout << " KF posteriori STATE" << kf_x;
// // // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?)
//cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state;
// kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?)
// // // ################## Geometric Transformation ######################################
// // ################## Geometric Transformation ######################################
// // for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori
// // {
// // //compute pseudorange posteriori estimation
// // // rho_est(i)=;
// // //compute LOS sat-receiver vector components posteriori
// // // a_x(i)=;
// // // a_y(i)=;
// // // a_z(i)=;
// // //compute pseudorange rate posteriori estimation
// // // rhoDot_est(i)=;
// // }
// for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori
// {
// //compute pseudorange posteriori estimation
// // rho_est(i)=;
// //compute LOS sat-receiver vector components posteriori
// // a_x(i)=;
// // a_y(i)=;
// // a_z(i)=;
// //compute pseudorange rate posteriori estimation
// // rhoDot_est(i)=;
// }
// // kf_H = arma::zeros(8, 2*new_data.sat_number);
// kf_H = arma::zeros(8, 2*new_data.sat_number);
// // for (int32_t i = 0; i < new_data.sat_number; n++) // Measurement matrix H posteriori assembling
// // {
// // // It has 8 columns (8 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, 6) = 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, 7) = 1.0;
// // }
// for (int32_t i = 0; i < new_data.sat_number; n++) // Measurement matrix H posteriori assembling
// {
// // It has 8 columns (8 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, 6) = 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, 7) = 1.0;
// }
// // //Re-calculate error measurement vector with the most recent data available
// // //kf_delta_y=kf_H*kf_delta_x
// // //Filtered pseudorange error measurement (in m):
// // //delta_rho_filt=;
// // //Filtered Doppler error measurement (in Hz):
// // //delta_doppler_filt=;
// //Re-calculate error measurement vector with the most recent data available
// //kf_delta_y=kf_H*kf_delta_x
// //Filtered pseudorange error measurement (in m):
// //delta_rho_filt=;
// //Filtered Doppler error measurement (in Hz):
// //delta_doppler_filt=;
// //TODO: Fill the tracking commands outputs
// // Notice: keep the same satellite order as in the Vtl_Data matrices
// // sample code
//TODO: Fill the tracking commands outputs
// Notice: keep the same satellite order as in the Vtl_Data matrices
// sample code
TrackingCmd trk_cmd;
trk_cmd.carrier_freq_hz = 0;
trk_cmd.carrier_freq_rate_hz_s = 0;

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

@ -20,6 +20,7 @@
#include "trackingcmd.h"
#include "vtl_conf.h"
#include "vtl_data.h"
#include "MATH_CONSTANTS.h"
#include <armadillo>
#include <cstdint>
#include <string>
@ -83,6 +84,8 @@ private:
arma::colvec kf_x; // state vector
arma::colvec kf_x_pre; // predicted state vector
arma::colvec kf_y; // measurement vector
arma::colvec kf_yerr; // ERROR measurement vector
arma::colvec kf_xerr; // ERROR state vector
arma::mat kf_K; // Kalman gain matrix
// Gaussian estimator