[ADD] static kalman version

src/algorithms/PVT/adapters/rtklib_pvt.cc

@@ -76,6 +76,7 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
 
     // PVT KF settings
     pvt_output_parameters.enable_pvt_kf = configuration->property(role + ".enable_pvt_kf", false);
+    pvt_output_parameters.static_scenario_sd = configuration->property(role + ".static_scenario_sd", false);
     pvt_output_parameters.estatic_measures_sd = configuration->property(role + ".estatic_measures_sd", false);
     pvt_output_parameters.measures_ecef_pos_sd_m = configuration->property(role + ".kf_measures_ecef_pos_sd_m", 1.0);
     pvt_output_parameters.measures_ecef_vel_sd_ms = configuration->property(role + ".kf_measures_ecef_vel_sd_ms", 0.1);

src/algorithms/PVT/libs/pvt_conf.h

@@ -97,6 +97,7 @@ public:
 
     // PVT KF parameters
     bool enable_pvt_kf = false;
+    bool static_scenario_sd = false;
     bool estatic_measures_sd = false;
     double measures_ecef_pos_sd_m = 1.0;
     double measures_ecef_vel_sd_ms = 0.1;

src/algorithms/PVT/libs/pvt_kf.cc

@@ -24,6 +24,7 @@ void Pvt_Kf::init_Kf(const arma::vec& p,
     const arma::vec& v,
     const arma::vec& res_pv,
     double solver_interval_s,
+    bool static_scenario_sd,
     bool estatic_measures_sd,
     double measures_ecef_pos_sd_m,
     double measures_ecef_vel_sd_ms,
@@ -32,6 +33,14 @@ void Pvt_Kf::init_Kf(const arma::vec& p,
 {
     // Kalman Filter class variables
     const double Ti = solver_interval_s;
+    if (static_scenario_sd)
+        {
+            scenario_static = true;
+        }
+    else
+        {
+            scenario_static = false;
+        }
 
     d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0},
         {0.0, 1.0, 0.0, 0.0, Ti, 0.0},
@@ -112,6 +121,7 @@ void Pvt_Kf::reset_Kf()
 
 void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_pv)
 {
+    // bool static_scenario = true;
     if (d_initialized)
         {
             // Kalman loop
@@ -132,7 +142,15 @@ void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res
                                 {0.0, 0.0, 0.0, res_pv[9], res_pv[7], res_pv[10]},
                                 {0.0, 0.0, 0.0, res_pv[11], res_pv[10], res_pv[8]}};
                         }
-                    arma::vec z = arma::join_cols(p, v);
+                    arma::vec z = arma::zeros(6);
+                    if (scenario_static)
+                        {
+                            d_x_new_old[3] = 0;
+                            d_x_new_old[4] = 0;
+                            d_x_new_old[5] = 0;
+                        }
+
+                    z = arma::join_cols(p, v);
                     arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R);  // Kalman gain
 
                     d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old);

src/algorithms/PVT/libs/pvt_kf.h

@@ -39,6 +39,7 @@ public:
         const arma::vec& v,
         const arma::vec& res_pv,
         double solver_interval_s,
+        bool static_scenario_sd,
         bool estatic_measures_sd,
         double measures_ecef_pos_sd_m,
         double measures_ecef_vel_sd_ms,
@@ -63,6 +64,7 @@ private:
     arma::vec d_x_new_new;
     bool d_initialized{false};
     bool d_static{false};
+    bool scenario_static{false};
 };
 
 

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -1592,6 +1592,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                         v,
                                         res_pv,
                                         kf_update_interval_s,
+                                        d_conf.static_scenario_sd,
                                         d_conf.estatic_measures_sd,
                                         d_conf.measures_ecef_pos_sd_m,
                                         d_conf.measures_ecef_vel_sd_ms,