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

ADD: 3DoF model linearization in F matrix

Browse Source
This commit is contained in:
M.A. Gomez 2023-06-18 22:48:02 +02:00
parent b68d8c5c7c
commit 352ded5036
2 changed files with 20 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -124,7 +124,6 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
//************************************** //**************************************
// Kalman state prediction (time update) // Kalman state prediction (time update)
//************************************** //**************************************
kf_F_fill(kf_F, kf_dt);
if (counter < closure_point) if (counter < closure_point)
{ {
@ -161,7 +160,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
// kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt; // kf_x(7) = (kf_x(7)-kf_dx(7))/kf_dt;
// kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt; // kf_x(8) = (kf_x(8)-kf_dx(8))/kf_dt;
} }
kf_F_fill(kf_F, kf_dt, kf_x);
obsv_calc(rho_pri, rhoDot_pri, rhoDot2_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x); obsv_calc(rho_pri, rhoDot_pri, rhoDot2_pri, a_x, a_y, a_z, new_data.sat_number, new_data.sat_p, new_data.sat_v, kf_x);
kf_P_x = 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
@ -299,8 +298,20 @@ void Vtl_Engine::kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arm
} }
} }
void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt) void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x)
{ {
double densidad = 1.0;
double ballistic_coef = 0.007;
double diam_cohete = 120.0e-3; // 120 mm
double mass_rocket = 50.0; // 50Kg
double beta = (GNSS_PI * densidad * diam_cohete * diam_cohete / 8) * ballistic_coef;
// modulo de la velocidad
double vx = kf_x(3);
double vy = kf_x(4);
double vz = kf_x(5);
double u = norm(kf_x.rows(3, 5), 2);
kf_F(0, 3) = kf_dt; kf_F(0, 3) = kf_dt;
kf_F(0, 6) = kf_dt * kf_dt / 2; kf_F(0, 6) = kf_dt * kf_dt / 2;
kf_F(1, 4) = kf_dt; kf_F(1, 4) = kf_dt;
@ -312,6 +323,10 @@ void Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt)
kf_F(4, 7) = kf_dt; kf_F(4, 7) = kf_dt;
kf_F(5, 8) = kf_dt; kf_F(5, 8) = kf_dt;
kf_F(6, 3) = -beta*(vx*vx/u+u);
kf_F(7, 4) = -beta*(vy*vy/u+u);
kf_F(8, 5) = -beta*(vz*vz/u+u);
kf_F(9, 10) = kf_dt; kf_F(9, 10) = kf_dt;
} }

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

@ -104,7 +104,7 @@ private:
double delta_t_cmd = 0; double delta_t_cmd = 0;
void 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 void 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
void kf_F_fill(arma::mat &kf_F, double kf_dt); // System Matrix constructor void kf_F_fill(arma::mat &kf_F, double kf_dt, arma::mat &kf_x); // System Matrix constructor
void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_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 void obsv_calc(arma::mat &rho_pri, arma::mat &rhoDot_pri, arma::mat &rhoDot2_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
void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x); void kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat rho_pri, arma::mat rhoDot_pri, arma::mat rhoDot2_pri, arma::colvec pr_m, arma::colvec doppler_hz, arma::mat kf_x);
void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter); void model3DoF(double &acc_x, double &acc_y, double &acc_z, arma::mat kf_x, double dt, int counter);