Add doxygen docs

src/ekf/ekf.cpp

@@ -538,13 +538,11 @@ void EKF::AugmentStateIfNeeded(unsigned int camera_id, int frame_id)
   } else {
     /// @todo(jhartzer): Evaluate switching to second element / creating map
     // Remove first element from state
-    m_state.aug_states.erase(
-      m_state.aug_states.begin());
+    m_state.aug_states.erase(m_state.aug_states.begin());
 
     // Remove first element from covariance
     m_cov = RemoveFromMatrix(
-      m_cov, cam_index + g_cam_state_size,
-      cam_index + g_cam_state_size, g_aug_state_size);
+      m_cov, cam_index + g_cam_state_size, cam_index + g_cam_state_size, g_aug_state_size);
   }
 
   RefreshIndices();

src/ekf/ekf.hpp

@@ -43,17 +43,17 @@ class EKF
   ///
   typedef struct Parameters
   {
-    std::shared_ptr<DebugLogger> debug_logger;                  ///< @brief
-    double body_data_rate{1.0};                                 ///< @brief
-    bool data_logging_on{false};                                ///< @brief
-    std::string log_directory{""};                              ///< @brief
-    AugmentationType augmenting_type{AugmentationType::ALL};    ///< @brief
-    double augmenting_time{1.0};                                ///< @brief
-    double augmenting_pos_error{0.1};                           ///< @brief
-    double augmenting_ang_error{0.1};                           ///< @brief
-    Eigen::VectorXd process_noise {Eigen::VectorXd::Ones(18)};  ///< @brief
-    Eigen::Vector3d pos_l_in_g {Eigen::Vector3d::Zero()};       ///< @brief
-    double ang_l_to_g{0.0};                                     ///< @brief
+    std::shared_ptr<DebugLogger> debug_logger;                  ///< @brief Debug logger
+    double body_data_rate{1.0};                                 ///< @brief Body data log rate
+    bool data_logging_on{false};                                ///< @brief Data logging flag
+    std::string log_directory{""};                              ///< @brief Data log directory
+    AugmentationType augmenting_type{AugmentationType::ALL};    ///< @brief Augmenting type
+    double augmenting_time{1.0};                                ///< @brief Augmenting time
+    double augmenting_pos_error{0.1};                           ///< @brief Augmenting pos error
+    double augmenting_ang_error{0.1};                           ///< @brief Augmenting ang error
+    Eigen::VectorXd process_noise {Eigen::VectorXd::Ones(18)};  ///< @brief Process noise
+    Eigen::Vector3d pos_l_in_g {Eigen::Vector3d::Zero()};       ///< @brief Local frame position
+    double ang_l_to_g{0.0};                                     ///< @brief Local frame heading
     GpsInitType gps_init_type {GpsInitType::CONSTANT};          ///< @brief GPS initialization type
     double gps_init_baseline_dist {100.0};  ///< @brief Minimum pos projection error
     double gps_init_pos_thresh {0.1};       ///< @brief Minimum ang projection error

src/ekf/update/fiducial_updater.cpp

@@ -232,7 +232,7 @@ void FiducialUpdater::UpdateEKF(
   }
 
   /// @todo(jhartzer): This doesn't account for angular errors. Apply transform to R?
-  double position_sigma = 3 * pos_error;  // / std::sqrt(board_track.size());
+  double position_sigma = 3 * pos_error * ang_error;  // / std::sqrt(board_track.size());
   Eigen::MatrixXd R = position_sigma * Eigen::MatrixXd::Identity(res_x.rows(), res_x.rows());
 
   // Apply Kalman update

src/ekf/update/gps_updater.hpp

@@ -69,17 +69,12 @@ class GpsUpdater : public Updater
     Eigen::Vector3d gps_lla);
 
   ///
-  /// @brief
-  /// @param ekf
-  /// @param gps_time_vec
-  /// @param gps_ecef_vec
-  /// @param local_xyz_vec
+  /// @brief Update/marginalize EKF using GPS measurements used to initialize local frame
+  /// @param ekf EKF pointer
   ///
   void MultiUpdateEKF(std::shared_ptr<EKF> ekf);
 
 private:
-  double m_pos_stddev {0.0};
-  double m_ang_stddev {0.0};
   bool m_is_extrinsic {false};
   DataLogger m_data_logger;
 };

src/infrastructure/sim/truth_engine.hpp

@@ -245,7 +245,7 @@ class TruthEngine
   void WriteTruthData(double body_data_rate, std::string output_directory);
 
   ///
-  /// @brief
+  /// @brief Feature generation function
   /// @param feature_count Number of features to generate
   /// @param room_size Size of room to distribute features in
   /// @param rng Random number generator to use in generation