Feature 150 Extended Kalman Filter

README.md

@@ -34,13 +34,14 @@ The following is an indicative list of examples.
 - <a href="examples/rl/rl_example_13/rl_example_13.md">REINFORCE algorithm on ```CartPole-v0```</a>
 - <a href="examples/rl/rl_example_14/rl_example_14.cpp">Expected SARSA on ```CliffWalking-v0```</a>
 - <a href="examples/example_15/example_15.cpp">Approximate Monte Carlo on ```MountainCar-v0```</a>
-- <a href="examples/example_16/example_16.cpp">Monte Carlo tree search on ```Taxi-v3```</a>
+- <a href="examples/rl_example_16/rl_example_16.md">Monte Carlo tree search on ```Taxi-v3```</a>
 - <a href="examples/rl/rl_example_18.cpp">Double Q-learning on  ```CartPole-v0``` </a>
 - <a href="examples/rl/rl_example_19/rl_example_19.cpp">First visit Monte Carlo on ```FrozenLake-v0```</a>
 
 ### Filtering
 
 - <a href="examples/filtering/filtering_example_1/filtering_example_1.md">Kalman filtering</a>
+- <a href="examples/filtering/filtering_example_2/filtering_example_2.md">Extended Kalman filter for diff-drive system</a>
 
 
 ### Path planning

examples/filtering/CMakeLists.txt

@@ -1,2 +1,3 @@
 ADD_SUBDIRECTORY(filtering_example_1)
+ADD_SUBDIRECTORY(filtering_example_2)
 

examples/filtering/filtering_example_2/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.6 FATAL_ERROR)
+
+SET(EXECUTABLE  filtering_example_2)
+SET(SOURCE ${EXECUTABLE}.cpp)
+
+ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
+
+TARGET_LINK_LIBRARIES(${EXECUTABLE} cubeailib)
+
+IF( USE_PYTORCH )
+    TARGET_LINK_LIBRARIES(${EXECUTABLE} ${TORCH_LIBRARIES})
+ENDIF()
+
+TARGET_LINK_LIBRARIES(${EXECUTABLE} pthread)
+TARGET_LINK_LIBRARIES(${EXECUTABLE} openblas)
+TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
+target_link_libraries(${EXECUTABLE} tbb)
+target_link_libraries(${EXECUTABLE} boost_log)

examples/filtering/filtering_example_2/filtering_example_2.cpp

@@ -0,0 +1,222 @@
+#include "cubeai/base/cubeai_types.h"
+#include "cubeai/estimation/extended_kalman_filter.h"
+#include "cubeai/utils/iteration_counter.h"
+#include "cubeai/io/csv_file_writer.h"
+#include "rlenvs/dynamics/diff_drive_dynamics.h"
+#include "rlenvs/dynamics/system_state.h"
+#include "rlenvs/utils/unit_converter.h"
+
+#include <boost/log/trivial.hpp>
+#include <iostream>
+#include <unordered_map>
+#include <any>
+#include <vector>
+#include <random>
+
+namespace example_2
+{
+
+using cubeai::real_t;
+using cubeai::uint_t;
+using cubeai::DynMat;
+using cubeai::DynVec;
+using cubeai::estimation::ExtendedKalmanFilter;
+using cubeai::utils::IterationCounter;
+using cubeai::io::CSVWriter;
+using rlenvscpp::dynamics::DiffDriveDynamics;
+using rlenvscpp::dynamics::SysState;
+
+
+class ObservationModel
+{
+
+public:
+
+	// the ExtendedKalmanFilter expects an exposed
+	// input_type
+    typedef  DynVec<real_t> input_type;
+
+    ObservationModel();
+
+    // simply return the state
+    const DynVec<real_t> evaluate(const DynVec<real_t>& input)const;
+
+    // get the H or M matrix
+    const DynMat<real_t>& get_matrix(const std::string& name)const;
+
+private:
+
+    DynMat<real_t> H;
+    DynMat<real_t> M;
+};
+
+ObservationModel::ObservationModel()
+    :
+      H(2, 3),
+      M(2, 2)
+{
+	H = DynMat<real_t>::Zero(2,3);
+	M = DynMat<real_t>::Zero(2,2);
+    H(0, 0) = 1.0;
+    H(1,1) = 1.0;
+    M(0,0) = 1.0;
+    M(1, 1) = 1.0;
+
+}
+
+const DynVec<real_t>
+ObservationModel::evaluate(const DynVec<real_t>& input)const{
+    return input;
+}
+
+const DynMat<real_t>&
+ObservationModel::get_matrix(const std::string& name)const{
+    if(name == "H"){
+        return H;
+    }
+    else if(name == "M"){
+        return M;
+    }
+
+    throw std::logic_error("Invalid matrix name. Name "+name+ " not found");
+}
+
+const DynVec<real_t> get_measurement(const SysState<3>& state){
+
+   DynVec<real_t> result(2);
+   result[0] = state.get("X");
+   result[1] = state.get("Y");
+   return result;
+}
+
+
+}
+
+int main() {
+
+    using namespace example_2;
+
+	BOOST_LOG_TRIVIAL(info)<<"Starting example...";
+
+	uint_t n_steps = 300;
+	auto time = 0.0;
+	auto dt = 0.5;
+
+	/// angular velocity
+	auto w = 0.0;
+
+	/// linear velocity
+	auto vt = 1.0;
+
+	std::array<real_t, 2> motion_control_error;
+	motion_control_error[0] = 0.0;
+	motion_control_error[1] = 0.0;
+
+	DiffDriveDynamics exact_motion_model;
+	exact_motion_model.set_matrix_update_flag(false);
+	exact_motion_model.set_time_step(dt);
+
+	DiffDriveDynamics motion_model;
+	motion_model.set_time_step(dt);
+
+	std::map<std::string, std::any> input;
+	input["v"] = 1.0;
+	input["w"] = 0.0;
+	input["errors"] = motion_control_error;
+	motion_model.initialize_matrices(input);
+
+	ObservationModel observation;
+
+	ExtendedKalmanFilter<DiffDriveDynamics, ObservationModel> ekf(motion_model, observation);
+
+	DynMat<real_t> R = DynMat<real_t>::Zero(2, 2);
+	R(0,0) = 1.0;
+	R(1, 1) = 1.0;
+
+	DynMat<real_t> Q = DynMat<real_t>::Zero(2, 2);
+	Q(0,0) = 0.001;
+	Q(1, 1) = 0.001;
+
+	DynMat<real_t> P = DynMat<real_t>::Zero(3, 3);
+	P(0, 0) = 1.0;
+	P(1, 1) = 1.0;
+	P(2, 2) = 1.0;
+
+	ekf.with_matrix("P", P)
+	   .with_matrix("R", R)
+	   .with_matrix("Q", Q);
+
+	CSVWriter writer("state");
+	writer.open();
+	std::vector<std::string> names{"Time", "X_true", "Y_true", "Theta_true", "X", "Y", "Theta"};
+	writer.write_column_names(names);
+
+    try{
+
+
+		BOOST_LOG_TRIVIAL(info)<<"Starting simulation... "<<time;
+        uint_t counter=0;
+
+        std::map<std::string, std::any> motion_input;
+        motion_input["v"] = vt; // we keep the velocity constant
+        motion_input["errors"] = motion_control_error;
+
+        for(uint_t step=0; step < n_steps; ++step){
+
+			BOOST_LOG_TRIVIAL(info)<<"Simulation time: "<<time;
+
+            if(counter == 50){
+              w = rlenvscpp::utils::unit_converter::degrees_to_rad(45.0);
+            }
+            else if(counter == 100){
+               w = rlenvscpp::utils::unit_converter::degrees_to_rad(-45.0);
+            }
+            else if(counter == 150){
+               w = rlenvscpp::utils::unit_converter::degrees_to_rad(-45.0);
+            }
+            else{
+                w = 0.0;
+            }
+
+            motion_input["w"] = w;
+
+            auto& exact_state = exact_motion_model.evaluate(motion_input);
+
+            ekf.predict(motion_input);
+
+            auto& state = motion_model.get_state();
+            auto z = get_measurement(state);
+            ekf.update(z);
+
+            BOOST_LOG_TRIVIAL(info)<<"Position: "<<ekf.get("X")<<", "<<ekf.get("Y");
+            BOOST_LOG_TRIVIAL(info)<<"Orientation: (degrees)"<<rlenvscpp::utils::unit_converter::rad_to_degrees(ekf.get("Theta"));
+            BOOST_LOG_TRIVIAL(info)<<"V: "<<vt<<", W: "<<w;
+
+            std::vector<real_t> row(7, 0.0);
+			row[0] = time;
+            row[1] = exact_state.get("X");
+            row[2] = exact_state.get("Y");
+			row[3] = exact_state.get("Theta");
+            row[4] = state.get("X");
+            row[5] = state.get("Y");
+			row[6] = state.get("Theta");
+            writer.write_row(row);
+
+            time += dt;
+            counter++;
+        }
+
+		BOOST_LOG_TRIVIAL(info)<<"Finished example...";
+    }
+    catch(std::runtime_error& e){
+        std::cerr<<e.what()<<std::endl;
+    }
+    catch(std::logic_error& e){
+        std::cerr<<e.what()<<std::endl;
+    }
+    catch(...){
+        std::cerr<<"Unknown exception was raised whilst running simulation."<<std::endl;
+    }
+
+   return 0;
+}