Wwu/alp 109 simulated mocked up robot manager (#18)

* ALP-62: Add RobotManager and EKF

* ALP-109: Add MockManger 2D for numerical sim

ALP-109: Add condtional compiles and docstrings

ALP-109: Add unit tests for mocked manager

ALP-109: Correct measurement prediction function

ALP-109: Add num sim script

ALP-109: Add mock manager unit tests

ALP-109: Correct unit tests, start jacobian

ALP-109: Finish testing mock measurement model

ALP-109: Retest after rebase

Generated Jacobian test points

ALP-109: Add example jacobian test

CMakeLists.txt

@@ -21,6 +21,7 @@ find_package (Eigen3 3.4 REQUIRED NO_MODULE)
 
 # testing set up
 option(BUILD_TESTS "Build Tests" OFF)
+option(USE_MOCK "Use Mocked Robot" OFF)
 if(BUILD_TESTS)
     find_package(Catch2 3 REQUIRED)
     include(CTest)

docs/Design-Doc.md

@@ -33,7 +33,7 @@ Based on the general algorithm, we form our implementation with the following co
 1. Mathematical Utilities: coordinate, measurement and pose structs with helper functions
 2. RobotManager: robot-facing utilities. Any sensor interfacing functions and robot control schemes will fall under this category/class
 3. EKF: template for all EKF instances; contains all necessary fields/methods to maintain landmark EKFs
-4. Particle Filter: simple class for a Raos-Blackwellised particle filter. Manages instances of EKF within particles and provides sampling utilities to update
+4. Particle Filter: simple class for a Rao-Blackwellised particle filter. Manages instances of EKF within particles and provides sampling utilities to update
 5. FastSLAM: general manager class that carries out the entire algorithm; interfaces with particle filter, EKFs, Robot manager, and any supporting services (e.g. simulation, visualization, etc).
 
 We will break down the design of each component in the next section.

docs/FastSLAM_SIM.m

@@ -0,0 +1,41 @@
+%% FastSLAM numerical simulation
+clear, clc, close all;
+
+%% Part 1: Measurement Prediction Function
+rob_starting_pose = [0 0 0]'; % x, y, heading
+landmark_loc = [0 1]';
+
+test_meas_1 = measFunc([1 0 0]', [0.02 1.11]');
+test_meas_2 = measFunc([1 0 0]', [1.76,2.15]');
+test_meas_3 = measFunc([1 0 0]', [0.78,-1.37]');
+test_meas_4 = measFunc([1 0 0]',[1.2, -0.7]');
+test_meas_5 = measFunc([1 0 0]',[-0.16,0]');
+test_meas_6 = measFunc([1 0 1.94]',[-0.16, 0]');
+
+%% Part 2: Measurement Model Prediction
+
+syms x_land y_land x_rob y_rob theta_rob
+
+range = sqrt((x_land - x_rob)^2 + (y_land - y_rob)^2);
+theta = atan((y_land - y_rob)/(x_land - x_rob)) - theta_rob;
+
+G = simplify(jacobian([range, theta],[x_land, y_land]));
+G(2,1) = -(y_land - y_rob) / ((y_land-y_rob)^2 + (x_land-x_rob)^2);
+G(2,2) = (x_land - x_rob) / ((y_land-y_rob)^2 + (x_land-x_rob)^2);
+
+G1 = eval(subs(G, [x_land, y_land, x_rob, y_rob, theta_rob], [1, 0, 1, 1, 0]))
+G2 = eval(subs(G, [x_land, y_land, x_rob, y_rob, theta_rob], [0, 1, 1, 0, 0]))
+G3 = eval(subs(G, [x_land, y_land, x_rob, y_rob, theta_rob], [0, 1, 1, 1, 0]))
+
+%% Function declarations
+
+function meas = measFunc(rob_pose, lm_pose)
+    meas(1) = norm(rob_pose(1:2) - lm_pose, 2);
+    reco_angle = atan((lm_pose(2) - rob_pose(2))/(lm_pose(1) - rob_pose(1)));
+    if (lm_pose(1) - rob_pose(1) < 0)
+        meas(2) = reco_angle + pi - rob_pose(3);
+        meas(2) = meas(2) - 2*pi*floor(meas(2)/pi);
+    else
+        meas(2) = reco_angle - rob_pose(3);
+    end
+end

include/core-structs.h

@@ -36,6 +36,11 @@ struct VelocityCommand2D {
    float wz_radps;    // Robot-frame angular velocity (radians per second)
 };
 
+/**
+ * @brief scoped enum for handling non-observation landmarks
+ */
+enum class NONE_OBS_LM {prediction = -1, unknown_lm = -2};
+
 /** An observation of a landmark as viewed from the robot's perspective. */
 struct Observation2D {
    float range_m;     // Range from the robot to the landmark (meters)

include/robot-manager.h

@@ -11,57 +11,210 @@
 #include <iostream>
 
 class RobotManager {
+
 public:
+    /**
+     * @brief samples from the robot IMU distribution, pure virtual function
+     */
     virtual void sampleIMU() = 0;
+
+    /**
+     * @brief obtain a landmark measurement from the appropriate sensor,
+     * pure virtual function
+     */
     virtual void sampleLandMark() = 0;
+
+    /**
+     * @brief get current robot control input, pure virtual function
+     */
     virtual void sampleControl() = 0;
 };
 
 class RobotManager2D : public RobotManager {
 
 protected:
+    /**
+     * @brief current robot pose member variable
+     */
     struct Pose2D m_curr_pose;
+
+    /**
+     * @brief current robot command input
+     */
     struct VelocityCommand2D m_curr_command;
+
+    /**
+     * @brief robot landmark sensor measurement noise, constant
+     */
     const Eigen::Matrix2f m_meas_noise;
+
+    /**
+     * @brief current landmark observation data (robot frame)
+     */
     struct Observation2D m_curr_obs;
 
+    /**
+     * @brief abstract class constructor to instantiate member variables
+     * only accessible to derived class
+     */
     RobotManager2D(struct Pose2D init_pose,
                    struct VelocityCommand2D init_cmd,
                    Eigen::Matrix2f meas_noise) : m_curr_pose(init_pose),
         m_curr_command(init_cmd), m_meas_noise(meas_noise){
         m_curr_obs = { .range_m = 0, .bearing_rad = 0 };
     }
-    RobotManager2D() {
-        m_curr_pose = {.x = 0, .y = 0, .theta_rad = 0};
-        m_curr_command = {.vx_mps = 0, .wz_radps = 0};
-    }
+
+    /**
+     * @brief virtual destructor, necessary for accessing derived class through base class pointer
+     */
     virtual ~RobotManager2D() {};
 
 public:
-    virtual void motionUpdate() = 0;
-    virtual struct Observation2D predictMeas() = 0;
+    /**
+     * @brief provide an update on robot trajectory given last pose
+     * @details this function is probabilistic, and the result is a sample from the motion model distribution
+     *
+     * @return a 2D pose struct sampled from motion model distribution
+     */
+    virtual struct Pose2D motionUpdate() = 0;
+
+    /**
+     * @brief predicts the measurement based on sensor's measurement model and robot trajectory
+     */
+    virtual struct Observation2D predictMeas(const struct Point2D mu_prev) = 0;
+
+    /**
+     * @brief get the latest robot observation
+     * @return a bearing-range measurement
+     */
     virtual struct Observation2D getCurrObs() const = 0;
+
+    /**
+     * @brief calculates the jacobian of the measurement function
+     *
+     * @param[in] mu_prev: previous landmark position belief
+     * @return a 2x2 matrix with eq conditions plugged in
+     */
     virtual Eigen::Matrix2f measJacobian(const struct Point2D mu_prev) const = 0;
+
+    /**
+     * @brief get current robot measurement noise
+     * @return a 2x2 sensor covariance matrix
+     */
     virtual Eigen::Matrix2f getMeasNoise() const = 0;
+
 };
 
 class Create3Manager final : public RobotManager2D {
 
 public:
-    Create3Manager();
+
+    Create3Manager() = delete;
+
+    /**
+     * @brief class constructor; robot initial conditions required
+     *
+     * @param[in] init_pose: robot starting pose
+     * @param[in] init_cmd: robot starting command
+     * @param[in] meas_noise: robot landmark sensor measurement noise (constant)
+     */
     Create3Manager(struct Pose2D init_pose,
                    struct VelocityCommand2D init_cmd,
                    Eigen::Matrix2f meas_noise): RobotManager2D(init_pose, init_cmd, meas_noise) {}
+
+    /**
+     * @brief default Create3 class destructor
+     */
     ~Create3Manager() {};
 
 
+    /**
+     * @brief get IMU reading from Create3 imu via ROS2
+     */
+    void sampleIMU() override;
+
+    /**
+     * @brief interface with LiDAR and get range-bearing reading on landmark
+     */
+    void sampleLandMark() override;
+
+    /**
+     * @brief get current control signal
+     */
+    void sampleControl() override;
+
+    /**
+     * @brief sample from Create3 motion model distribution
+     *
+     * @return a 2D pose struct sampled from the create3 pose distribution
+     */
+    struct Pose2D motionUpdate() override;
+
+    /**
+     * @brief get current landmark observation from Create3
+     *
+     * @return one landmark observation
+     */
+    struct Observation2D getCurrObs() const override;
+
+    /**
+     * @brief predict landmark measurement given robot states
+     * @details estimation is conditioned on previous landmark position and robot pose
+     *
+     * @return a "proposed" landmark observation
+     */
+    struct Observation2D predictMeas(const struct Point2D mu_prev) override;
+
+    /**
+     * @brief calculates the jacobian of the create3 landmark measurement model
+     *
+     * @param[in] mu_prev: previous landmark position belief
+     * @return a 2x2 jacobian matrix given robot pose and previous landmark pose
+     */
+    Eigen::Matrix2f measJacobian(const struct Point2D mu_prev) const override;
+
+    /**
+     * @brief get Create3 sensor measurement noise
+     */
+    Eigen::Matrix2f getMeasNoise() const override;
+};
+
+#ifdef USE_MOCK
+class MockManager2D final: public RobotManager2D {
+
+public:
+
+    MockManager2D() = delete;
+
+    MockManager2D(struct Pose2D init_pose,
+                   struct VelocityCommand2D init_cmd,
+                   Eigen::Matrix2f meas_noise): RobotManager2D(init_pose, init_cmd, meas_noise) {}
+
+    ~MockManager2D() {};
+
     void sampleIMU() override;
+
     void sampleLandMark() override;
+
     void sampleControl() override;
-    void motionUpdate() override;
+
+    struct Pose2D motionUpdate() override;
+
     struct Observation2D getCurrObs() const override;
-    struct Observation2D predictMeas() override;
+
+    struct Observation2D predictMeas(const struct Point2D mu_prev) override;
+
     Eigen::Matrix2f measJacobian(const struct Point2D mu_prev) const override;
+
     Eigen::Matrix2f getMeasNoise() const override;
 
+
+    // test commands
+    void setObs(const struct Observation2D& new_obs);
+
+    void setControl(const struct VelocityCommand2D& new_ctrl);
+
+    void setState(const struct Pose2D& new_pose);
+
 };
+#endif // USE_MOCK

src/FastSLAM/CMakeLists.txt

@@ -1,8 +1,11 @@
 add_library(FastSLAMLib
    math-util.cpp
    EKF.cpp
-   robot-manager.cpp
+   create3-manager.cpp
 )
+if(USE_MOCK)
+    target_sources(FastSLAMLib PUBLIC mock-manager2d.cpp)
+endif()
 
 target_include_directories(FastSLAMLib PUBLIC
                            "${PROJECT_BINARY_DIR}"
@@ -23,6 +26,22 @@ if(BUILD_TESTS)
   )
 
   add_executable(test_EKF EKF_test.cpp)
+
+  if(USE_MOCK)
+    target_compile_definitions(test_EKF PUBLIC USE_MOCK)
+    add_executable(test_MockManager2d mock-manager2d_test.cpp)
+    target_compile_definitions(test_MockManager2d PUBLIC USE_MOCK)
+    target_link_libraries(test_MockManager2d
+                        PRIVATE Catch2::Catch2WithMain
+                        FastSLAMLib)
+    catch_discover_tests(test_MockManager2d)
+    target_include_directories(test_MockManager2d PUBLIC
+      "${PROJECT_BINARY_DIR}"
+      "${PROJECT_SOURCE_DIR}/include"
+    )
+  else()
+    target_compile_definitions(test_EKF PUBLIC USE_SIM)
+  endif()
   target_link_libraries(test_EKF
                         PRIVATE Catch2::Catch2WithMain
                         FastSLAMLib)

src/FastSLAM/EKF.cpp

@@ -56,7 +56,7 @@ KF_RET LMEKF2D::update() {
         Eigen::Matrix2f K = this->calcKalmanGain();
         Eigen::Matrix2f G_n = this->measJacobian();
 
-        m_mu += K * ( m_robot->getCurrObs() - m_robot->predictMeas() );
+        m_mu += K * ( m_robot->getCurrObs() - m_robot->predictMeas(m_mu) );
         m_sigma = ( Eigen::Matrix2f::Identity() - K * G_n ) * m_sigma;
     }
 
@@ -73,7 +73,7 @@ float LMEKF2D::calcCPD() {
         return -1.0f;
     }
 
-    Eigen::Vector2f residue = m_robot->getCurrObs() - m_robot->predictMeas();
+    Eigen::Vector2f residue = m_robot->getCurrObs() - m_robot->predictMeas(m_mu);
     float weight = sqrtf( (2 * M_PI * m_meas_cov).determinant() );
     weight = weight * expf( -0.5 * residue.transpose() * m_meas_cov.inverse() * residue );
 

src/FastSLAM/EKF_test.cpp

@@ -39,12 +39,18 @@ TEST_CASE("Empty EKF"){
 TEST_CASE("Non-empty EKF") {
     // set-up
     struct Pose2D init_pose = { .x = 0, .y =0, .theta_rad = 0 };
-    struct VelocityCommand2D init_cmd = { .vx_mps = 0, .wz_radps = 0 };
-    struct Point2D init_obs = { .x = 1, .y = 0 };
+    struct VelocityCommand2D init_cmd = { .vx_mps = 1, .wz_radps = 0 };
+    struct Point2D init_obs = { .x = 0, .y = 1 };
     Eigen::Matrix2f init_cov;
     init_cov << 1, 0,
         0, 1;
+
+#ifdef USE_MOCK
+    std::shared_ptr<RobotManager2D> robot_instance = std::make_shared<MockManager2D>(init_pose, init_cmd, init_cov);
+    std::unique_ptr<MockManager2D> test_manager = std::make_unique<MockManager2D>(init_pose, init_cmd, init_cov);
+#elif defined(USE_SIM)
     std::shared_ptr<RobotManager2D> robot_instance = std::make_shared<Create3Manager>(init_pose, init_cmd, init_cov);
+#endif
 
     LMEKF2D* filled_landmark_ekf = new LMEKF2D(init_obs, init_cov, robot_instance);
     REQUIRE(robot_instance.use_count() == 2); // check correct ownership changes
@@ -61,8 +67,11 @@ TEST_CASE("Non-empty EKF") {
 
     }
 
+    //TODO: needs numerical sim and mock manager
     SECTION("Test: one step updates correctly") {
-        // TODO: requires mock-up robot manager
+#ifdef USE_MOCK
+        test_manager->sampleIMU();
+#endif //USE_MOCK
     }
 
     SECTION("Test: calculation of correct observation correspondence") {