mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-05-07 09:54:10 +00:00
254 lines
10 KiB
C++
Executable File
254 lines
10 KiB
C++
Executable File
/*!
|
|
* \file vtl_engine.h
|
|
* \brief Class that implements a Vector Tracking Loop (VTL) Kalman filter engine
|
|
* \author Javier Arribas, 2022. jarribas(at)cttc.es
|
|
*
|
|
* -----------------------------------------------------------------------------
|
|
*
|
|
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
|
* This file is part of GNSS-SDR.
|
|
*
|
|
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
|
|
* SPDX-License-Identifier: GPL-3.0-or-later
|
|
*
|
|
* -----------------------------------------------------------------------------
|
|
*/
|
|
|
|
#include "vtl_engine.h"
|
|
#include "iostream"
|
|
|
|
using namespace std;
|
|
|
|
Vtl_Engine::Vtl_Engine()
|
|
{
|
|
}
|
|
|
|
Vtl_Engine::~Vtl_Engine()
|
|
{
|
|
}
|
|
|
|
bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
|
|
{
|
|
//TODO: Implement main VTL loop here
|
|
using arma::as_scalar;
|
|
// ################## Kalman filter initialization ######################################
|
|
// covariances (static)
|
|
kf_P_x = arma::eye(8, 8); //TODO: use a real value.
|
|
kf_x = arma::zeros(8, 1);
|
|
kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number);
|
|
double kf_dt=1e-1;
|
|
kf_Q = arma::zeros(8, 8);
|
|
|
|
kf_F = arma::zeros(8, 8);
|
|
kf_F(0, 0) = 1.0; kf_F(0, 3) = kf_dt;
|
|
kf_F(1, 1) = 1.0; kf_F(1, 4) = kf_dt;
|
|
kf_F(2, 2) = 1.0; kf_F(2, 5) = kf_dt;
|
|
kf_F(3, 3) = 1.0;
|
|
kf_F(4, 4) = 1.0;
|
|
kf_F(5, 5) = 1.0;
|
|
kf_F(6, 6) = 1.0; kf_F(6, 7) = kf_dt;
|
|
kf_F(7, 7) = 1.0;
|
|
|
|
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 ######################################
|
|
// receiver solution from rtklib_solver
|
|
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)=new_data.rx_dts(0);
|
|
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)
|
|
//kf_x.print(" KF RTKlib STATE");
|
|
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);
|
|
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);
|
|
rhoDot_pri = 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_z = arma::zeros(new_data.sat_number, 1);
|
|
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)+cdeltatDot_u;
|
|
}
|
|
|
|
kf_H = arma::zeros(2*new_data.sat_number,8);
|
|
|
|
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H 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;
|
|
}
|
|
// Kalman estimation (measurement update)
|
|
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector
|
|
{
|
|
//kf_y(i) = new_data.pr_m(i); // i-Satellite
|
|
//kf_y(i+new_data.sat_number) = rhoDot_pri(i)/Lambda_GPS_L1; // i-Satellite
|
|
kf_yerr(i)=new_data.pr_m(i)-rho_pri(i);//-0.000157*SPEED_OF_LIGHT_M_S;
|
|
kf_yerr(i+new_data.sat_number)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-rhoDot_pri(i);
|
|
|
|
}
|
|
kf_yerr.print("KF measurement vector difference");
|
|
|
|
// DOUBLES DIFFERENCES
|
|
// kf_yerr = arma::zeros(2*new_data.sat_number, 1);
|
|
// for (int32_t i = 1; i < new_data.sat_number; i++) // Measurement vector
|
|
// {
|
|
// kf_y(i)=new_data.pr_m(i)-new_data.pr_m(i-1);
|
|
// kf_yerr(i)=kf_y(i)-(rho_pri(i)+rho_pri(i-1));
|
|
// kf_y(i+new_data.sat_number)=(rhoDot_pri(i)-rhoDot_pri(i-1))/Lambda_GPS_L1;
|
|
// kf_yerr(i+new_data.sat_number)=kf_y(i+new_data.sat_number)-(new_data.doppler_hz(i)-new_data.doppler_hz(i-1));
|
|
// }
|
|
// kf_yerr.print("DOUBLES DIFFERENCES");
|
|
|
|
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)
|
|
kf_R(i, i) = 0.1; //TODO: fill with real values.
|
|
kf_R(i+new_data.sat_number, i+new_data.sat_number) = 1.0;
|
|
}
|
|
|
|
// 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
|
|
|
|
|
|
// States related tu USER clock adjust from m/s to s (by /SPEED_OF_LIGHT_M_S)
|
|
|
|
kf_x(6) =kf_x(6) /SPEED_OF_LIGHT_M_S;
|
|
kf_x(7) =kf_x(7) /SPEED_OF_LIGHT_M_S;
|
|
new_data.kf_state.print(" KF RTKlib STATE");
|
|
cout << " KF posteriori STATE diference" << kf_x-new_data.kf_state;
|
|
cout << " KF posteriori STATE diference %" << (kf_x-new_data.kf_state)/new_data.kf_state*100;
|
|
|
|
// // ################## 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)*SPEED_OF_LIGHT_M_S;
|
|
//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)+kf_x(7)*SPEED_OF_LIGHT_M_S;
|
|
}
|
|
|
|
kf_H = arma::zeros(2*new_data.sat_number,8);
|
|
|
|
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H 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
|
|
kf_yerr=kf_H*kf_xerr;
|
|
// Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz):
|
|
|
|
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector
|
|
{
|
|
|
|
rho_pri(i)=new_data.pr_m(i)-kf_yerr(i); // now filtered
|
|
rhoDot_pri(i)=(new_data.doppler_hz(i)*Lambda_GPS_L1+cdeltatDot_u)-kf_yerr(i+new_data.sat_number); // now filtered
|
|
// TO DO: convert rhoDot_pri to doppler shift!
|
|
// Doppler shift defined as pseudorange rate measurement divided by the negative of carrier wavelength.
|
|
rhoDot_pri(i)=-rhoDot_pri(i)/Lambda_GPS_L1;
|
|
}
|
|
//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;
|
|
trk_cmd.code_freq_chips = 0;
|
|
trk_cmd.enable_carrier_nco_cmd = true;
|
|
trk_cmd.enable_code_nco_cmd = true;
|
|
trk_cmd.sample_counter = new_data.sample_counter;
|
|
trk_cmd_outs.push_back(trk_cmd);
|
|
new_data.debug_print();
|
|
return true;
|
|
}
|
|
|
|
void Vtl_Engine::reset()
|
|
{
|
|
//TODO
|
|
}
|
|
|
|
void Vtl_Engine::debug_print()
|
|
{
|
|
//TODO
|
|
}
|
|
|
|
void Vtl_Engine::configure(Vtl_Conf config_)
|
|
{
|
|
config = config_;
|
|
//TODO: initialize internal variables
|
|
}
|