mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-15 14:25:00 +00:00
ADD: first prototype of VTL KF
This commit is contained in:
M.A.Gomez
parent
b9394bd79d
commit
3c8a4f3d3f
@ -27,44 +27,46 @@ Vtl_Engine::~Vtl_Engine()
|
|||||||
bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
|
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::dot;
|
||||||
// // ################## Kalman filter initialization ######################################
|
// // ################## Kalman filter initialization ######################################
|
||||||
|
|
||||||
// // covariances (static)
|
// // covariances (static)
|
||||||
// kf_P_x_ini = arma::zeros(8, 8);
|
kf_P_x_ini = arma::zeros(8, 8); //TODO: use a real value.
|
||||||
// kf_x_pri = arma::zeros(8, 1);
|
kf_x = arma::zeros(8, 1);
|
||||||
// kf_R = arma::zeros(2*n_sats, 2*n_sats);
|
kf_R = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number);
|
||||||
// kf_dt=1e-3;
|
double kf_dt=1e-3;
|
||||||
// kf_Q = arma::zeros(8, 8);
|
kf_Q = arma::zeros(8, 8);
|
||||||
|
|
||||||
// kf_F = arma::zeros(8, 8);
|
kf_F = arma::zeros(8, 8);
|
||||||
// kf_F(0, 0) = 1.0; kf_F(0, 3) = kf_dt;
|
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(1, 1) = 1.0; kf_F(1, 4) = kf_dt;
|
||||||
// kf_F(2, 2) = 1.0; kf_F(2, 5) = kf_dt;
|
kf_F(2, 2) = 1.0; kf_F(2, 5) = kf_dt;
|
||||||
// kf_F(3, 3) = 1.0;
|
kf_F(3, 3) = 1.0;
|
||||||
// kf_F(4, 4) = 1.0;
|
kf_F(4, 4) = 1.0;
|
||||||
// kf_F(5, 5) = 1.0;
|
kf_F(5, 5) = 1.0;
|
||||||
// kf_F(6, 6) = 1.0; kf_F(6, 7) = kf_dt;
|
kf_F(6, 6) = 1.0; kf_F(6, 7) = kf_dt;
|
||||||
// kf_F(7, 7) = 1.0;
|
kf_F(7, 7) = 1.0;
|
||||||
|
|
||||||
// kf_H = arma::zeros(8, 2*n_sats);
|
kf_H = arma::zeros(8, 2*new_data.sat_number);
|
||||||
// kf_x = arma::zeros(8, 1);
|
kf_x = arma::zeros(8, 1);
|
||||||
// kf_y = arma::zeros(2*n_sats, 1);
|
kf_y = arma::zeros(2*new_data.sat_number, 1);
|
||||||
// kf_P_y = arma::zeros(2*n_sats, 2*n_sats);
|
kf_S = arma::zeros(2*new_data.sat_number, 2*new_data.sat_number); // kf_P_y innovation covariance matrix
|
||||||
|
|
||||||
// // ################## Kalman Tracking ######################################
|
// // ################## Kalman Tracking ######################################
|
||||||
// // receiver solution from rtklib_solver
|
// // receiver solution from rtklib_solver
|
||||||
// kf_x(0)=new_vtl_data.rx_p(0);
|
kf_x(0)=new_data.rx_p(0);
|
||||||
// kf_x(1)=new_vtl_data.rx_p(1);
|
kf_x(1)=new_data.rx_p(1);
|
||||||
// kf_x(2)=new_vtl_data.rx_p(2);
|
kf_x(2)=new_data.rx_p(2);
|
||||||
// kf_x(3)=new_vtl_data.rx_v(0);
|
kf_x(3)=new_data.rx_v(0);
|
||||||
// kf_x(4)=new_vtl_data.rx_v(1);
|
kf_x(4)=new_data.rx_v(1);
|
||||||
// kf_x(5)=new_vtl_data.rx_v(2);
|
kf_x(5)=new_data.rx_v(2);
|
||||||
// kf_x(6)=new_vtl_data.rx_dts(0);
|
kf_x(6)=new_data.rx_dts(0);
|
||||||
// kf_x(7)=new_vtl_data.rx_dts(1);
|
kf_x(7)=new_data.rx_dts(1);
|
||||||
|
|
||||||
// // Kalman state prediction (time update)
|
// // Kalman state prediction (time update)
|
||||||
// kf_x_pri = kf_F * kf_x; // state prediction
|
kf_x = kf_F * kf_x; // state prediction
|
||||||
// //kf_P_x_pri = kf_F * kf_P_x * kf_F.t() + kf_Q; // state error covariance 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
|
||||||
// //x_u=x_u0+kf_x_pri(0);
|
// //x_u=x_u0+kf_x_pri(0);
|
||||||
@ -75,70 +77,81 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
|
|||||||
// //zDot_u=zDot_u0+kf_x_pri(5);
|
// //zDot_u=zDot_u0+kf_x_pri(5);
|
||||||
// //cdeltat_u=cdeltat_u0+kf_x_pri(6);
|
// //cdeltat_u=cdeltat_u0+kf_x_pri(6);
|
||||||
// //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7);
|
// //cdeltatDot_u=cdeltatDot_u+kf_x_pri(7);
|
||||||
// //from state variables definition
|
// From state variables definition
|
||||||
// x_u=kf_x_pri(0);
|
x_u=kf_x(0);
|
||||||
// y_u=kf_x_pri(1);
|
y_u=kf_x(1);
|
||||||
// z_u=kf_x_pri(2);
|
z_u=kf_x(2);
|
||||||
// xDot_u=kf_x_pri(3);
|
xDot_u=kf_x(3);
|
||||||
// yDot_u=kf_x_pri(4);
|
yDot_u=kf_x(4);
|
||||||
// zDot_u=kf_x_pri(5);
|
zDot_u=kf_x(5);
|
||||||
// cdeltat_u=kf_x_pri(6);
|
cdeltat_u=kf_x(6);
|
||||||
// cdeltatDot_u=kf_x_pri(7);
|
cdeltatDot_u=kf_x(7);
|
||||||
// for (int32_t i = 0; i < n_sats; n++) //neccesary quantities
|
|
||||||
// {
|
|
||||||
// d(i)=sqrt(square(new_vtl_data.sat_p(i, 0)-x_u)+square(new_vtl_data.sat_p(i, 1)-y_u)+square(new_vtl_data.sat_p(i, 2)-z_u));
|
|
||||||
// //compute pseudorange estimation
|
|
||||||
// rho_pri(i)=d(i)+cdeltat_u;
|
|
||||||
// //compute LOS sat-receiver vector components
|
|
||||||
// a_x(i)=-(new_vtl_data.sat_p(i, 0)-x_u)/d(i);
|
|
||||||
// a_y(i)=-(new_vtl_data.sat_p(i, 1)-y_u)/d(i);;
|
|
||||||
// a_z(i)=-(new_vtl_data.sat_p(i, 2)-z_u)/d(i);;
|
|
||||||
// //compute pseudorange rate estimation
|
|
||||||
// rhoDot_pri(i)=(new_vtl_data.sat_v(i, 0)-xDot_u)*a_x(i)+(new_vtl_data.sat_v(i, 1)-yDot_u)*a_y(i)+(new_vtl_data.sat_v(i, 2)-zDot_u)*a_z(i)+cdeltatDot_u;
|
|
||||||
// }
|
|
||||||
|
|
||||||
// kf_H = arma::zeros(8, 2*n_sats);
|
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 < n_sats; n++) // Measurement matrix H assembling
|
for (int32_t i = 0; i < new_data.sat_number; i++) //neccesary quantities
|
||||||
// {
|
{
|
||||||
// // It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
|
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)));
|
||||||
// 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;
|
//compute pseudorange estimation
|
||||||
// kf_H(i+n_sats, 3) = a_x(i); kf_H(i+n_sats, 4) = a_y(i); kf_H(i+n_sats, 5) = a_z(i); kf_H(i+n_sats, 7) = 1.0;
|
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);;
|
||||||
|
//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;
|
||||||
|
}
|
||||||
|
|
||||||
// // Kalman estimation (measurement update)
|
kf_H = arma::zeros(8, 2*new_data.sat_number);
|
||||||
// for (int32_t i = 0; i < n_sats; n++) // Measurement vector
|
|
||||||
// {
|
|
||||||
// kf_y(i) = delta_rho(i); // i-Satellite
|
|
||||||
// kf_y(i+n_sats) = delta_rhoDot(i); // i-Satellite
|
|
||||||
// }
|
|
||||||
|
|
||||||
// for (int32_t i = 0; i < n_sats; n++) // Measurement error Covariance Matrix R assembling
|
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement matrix H assembling
|
||||||
// {
|
{
|
||||||
// // It is diagonal 2*NSatellite x 2*NSatellite (NSat psudorange error;NSat pseudo range rate error)
|
// It has 8 columns (8 states) and 2*NSat rows (NSat psudorange error;NSat pseudo range rate error)
|
||||||
// kf_R(i, i) = 1.0;
|
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_R(i+n_sats, i+n_sats) = 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 filter update step
|
// Kalman estimation (measurement update)
|
||||||
// // kf_P_y = kf_H * kf_P_x_pri * kf_H.t() + kf_R; // innovation covariance matrix (S)
|
for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector
|
||||||
// // kf_K = (kf_P_x_pri * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain
|
{
|
||||||
|
//kf_y(i) = delta_rho(i); // i-Satellite
|
||||||
|
kf_y(i)=new_data.pr_m(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);
|
||||||
|
}
|
||||||
|
|
||||||
|
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) = 1.0; //TODO: use a real value.
|
||||||
|
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
|
||||||
|
|
||||||
// for (int32_t i = 0; i < n_sats; n++) //Error measurement vector
|
//for (int32_t i = 0; i < new_data.sat_number; i++) //Error measurement vector
|
||||||
// {
|
//{
|
||||||
// // kf_delta_y(i)=rho(i)+delta_rho(i)-rho_pri(i); // pseudorange error
|
// kf_delta_y(i)=new_data.pr_m(i)+delta_rho(i)-rho_pri(i); // pseudorange error
|
||||||
// // kf_delta_y(i+n_sats)=rhoDot(i)+delta_F*(-lambdaC)-rhoDot_pri(i); // pseudorange rate error
|
// kf_delta_y(i+new_data.sat_number)=new_data.doppler_hz(i)+delta_F*(-lambdaC)-rhoDot_pri(i); // pseudorange rate error
|
||||||
// }
|
//}
|
||||||
|
|
||||||
// // kf_delta_x = kf_K * kf_delta_y; // updated error state estimation
|
//kf_delta_x = kf_K * kf_delta_y; // updated error state estimation
|
||||||
// // kf_P_x = (arma::eye(size(kf_P_x_pri)) - kf_K * kf_H) * kf_P_x_pri; // update state estimation error covariance matrix
|
kf_x = kf_K * (kf_y-dot(kf_H,kf_x)); // updated error state estimation
|
||||||
|
kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix
|
||||||
|
|
||||||
// // kf_x = kf_x_pri+kf_delta_x; // compute PVT from priori and error estimation (neccesary?)
|
// // 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 < n_sats; n++) //neccesary quantities at posteriori
|
// for (int32_t i = 0; i < new_data.sat_number; n++) //neccesary quantities at posteriori
|
||||||
// {
|
// {
|
||||||
// //compute pseudorange posteriori estimation
|
// //compute pseudorange posteriori estimation
|
||||||
// // rho_est(i)=;
|
// // rho_est(i)=;
|
||||||
@ -150,13 +163,13 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
|
|||||||
// // rhoDot_est(i)=;
|
// // rhoDot_est(i)=;
|
||||||
// }
|
// }
|
||||||
|
|
||||||
// kf_H = arma::zeros(8, 2*n_sats);
|
// kf_H = arma::zeros(8, 2*new_data.sat_number);
|
||||||
|
|
||||||
// for (int32_t i = 0; i < n_sats; n++) // Measurement matrix H posteriori assembling
|
// 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)
|
// // 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, 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+n_sats, 3) = a_x(i); kf_H(i+n_sats, 4) = a_y(i); kf_H(i+n_sats, 5) = a_z(i); kf_H(i+n_sats, 7) = 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
|
// //Re-calculate error measurement vector with the most recent data available
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user