Add new examples (#169)

Author: pockerman

examples/example_21/CMakeLists.txt

@@ -0,0 +1,24 @@
+CMAKE_MINIMUM_REQUIRED(VERSION 3.20 FATAL_ERROR)
+
+SET(EXECUTABLE  example_21)
+SET(SOURCE ${EXECUTABLE}.cpp)
+
+ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
+
+TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE bitrllib pthread boost_log)
+
+IF(ENABLE_CHRONO)
+    TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE Chrono::Chrono_core)
+ENDIF ()
+
+IF(BITRL_WEBOTS)
+    TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)
+ENDIF()
+
+IF(BITRL_MQTT)
+    TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE paho-mqttpp3 paho-mqtt3as)
+ENDIF ()
+
+IF(BITRL_OPENCV)
+    TARGET_LINK_LIBRARIES(${EXECUTABLE} PRIVATE ${OpenCV_LIBS})
+ENDIF ()

examples/example_21/example_21.cpp

@@ -0,0 +1,195 @@
+/**
+  * KalmanFilter example. The example is taken from the 
+  * paper <a href="https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf">An Introduction to the Kalman Filter</a> by
+  * Greg Welch and Gary Bishop
+  * */
+
+#include "bitrl/bitrl_types.h"
+#include "bitrl/estimation/kalman_filter.h"
+#include "bitrl/estimation/kf_model_base.h"
+#include "bitrl/utils/iteration_counter.h"
+#include "bitrl/utils/io/csv_file_writer.h"
+
+#define BOOST_LOG_DYN_LINK
+#include <boost/log/trivial.hpp>
+#include <iostream>
+#include <unordered_map>
+#include <any>
+#include <vector>
+#include <random>
+
+namespace example_1
+{
+
+using bitrl::real_t;
+using bitrl::uint_t;
+using bitrl::DynMat;
+using bitrl::DynVec;
+using bitrl::estimation::KalmanFilterConfig;
+using bitrl::estimation::KalmanFilter;
+using bitrl::estimation::KFMotionModelBase;
+using bitrl::estimation::KFModelBase;
+using bitrl::utils::IterationCounter;
+using bitrl::utils::io::CSVWriter;
+
+
+real_t DT = 1.0;
+real_t SIM_TIME = 50.0;
+
+struct Cmd
+{
+	// v: [m/s] omega: [rad/s]
+	static DynVec<real_t> cmd();
+};
+
+DynVec<real_t> 
+Cmd::cmd(){
+	
+	DynVec<real_t> u(1);
+	u << 0.0;
+    return u;
+}
+
+// simple struct that describes the Motion of the robots
+struct MotionModel: public KFMotionModelBase<DynMat<real_t>, DynVec<real_t>>
+{
+	const static uint_t VEC_SIZE;
+	
+};
+
+const uint_t MotionModel::VEC_SIZE = 1;
+
+// simple struct that describes the Observation model
+struct ObservationModel: public KFModelBase<DynMat<real_t>>
+{
+	const static uint_t VEC_SIZE;
+	static DynVec<real_t> sensors();
+};
+
+const uint_t ObservationModel::VEC_SIZE = 1;
+
+const real_t STD = 0.1;
+const real_t MU = -0.37727;
+
+DynVec<real_t> 
+ObservationModel::sensors(){ 
+	DynVec<real_t> all_sensors(1);
+	
+	std::normal_distribution<real_t> d{MU , STD};
+	std::mt19937 generator; //(42);
+	all_sensors << d(generator);
+	return all_sensors;
+}
+
+
+typedef MotionModel motion_model_type;
+typedef ObservationModel obs_motion_type;
+}
+
+int main() {
+
+    using namespace example_1;
+
+    try{
+		
+		typedef motion_model_type::state_type state_type;
+		obs_motion_type obs;
+		DynMat<real_t> H(ObservationModel::VEC_SIZE, MotionModel::VEC_SIZE);
+		H << 1.0;
+		
+		obs.add_matrix("H", H);
+		
+		// the motion model
+		motion_model_type motion;
+		
+		// set the initial state
+		DynVec<real_t> x_init(MotionModel::VEC_SIZE);
+		x_init << 0;
+		motion.set_state(x_init);
+		
+		//...and  the matrix describing the motion dynamics
+		DynMat<real_t> F(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+		F << 1.0;
+		
+		motion.add_matrix("F", F);
+		
+		KalmanFilterConfig<motion_model_type, obs_motion_type> kf_config;
+		kf_config.motion_model = &motion;
+		kf_config.observation_model = &obs;
+		
+		// the KalmanFilter to use
+		KalmanFilter<motion_model_type, obs_motion_type> kf(kf_config);
+		
+		// set up the matrices
+		DynMat<real_t> P(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+		P << 1.0;
+		
+		DynMat<real_t> Q(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+		Q << 1.0e-5;
+			 
+		DynMat<real_t> R(ObservationModel::VEC_SIZE, ObservationModel::VEC_SIZE);
+		R << STD*STD;
+		
+		DynMat<real_t> B(MotionModel::VEC_SIZE, MotionModel::VEC_SIZE);
+		B << 0.0;
+		
+		kf.with_matrix("P", P)
+		  .with_matrix("Q", Q)
+		  .with_matrix("R", R)
+		  .with_matrix("B", B);
+		
+		
+		// the input to the filter
+		std::map<std::string, std::any > kf_input; 
+		
+		auto n_steps = static_cast<uint_t>(SIM_TIME / DT);
+		
+		BOOST_LOG_TRIVIAL(info)<<"Expected number of time steps: "<<n_steps;
+		std::vector<state_type> rows;
+		rows.reserve(n_steps);
+		
+		auto current_time = 0.0;
+		while(current_time <= SIM_TIME){
+			
+			auto u = Cmd::cmd();
+			auto w = DynVec<real_t>(u.size());
+			w << 0.0;
+			auto z = ObservationModel::sensors();
+			
+			kf_input["u"] = u;
+			kf_input["w"] = w;
+			kf_input["z"] = z;
+			
+			auto& state_vec = kf.estimate(kf_input);
+			
+			BOOST_LOG_TRIVIAL(info)<<"Time: "<<current_time<<" solution "<<state_vec[0];
+			
+			state_type row(1 + state_vec.size());
+			row[0] = current_time;
+			
+			for(uint_t i =0; i < static_cast<uint_t>(state_vec.size()); ++i){
+				row[i + 1] = state_vec[i];
+			}
+			
+			rows.push_back(row);
+			current_time += DT;
+		}
+		
+		CSVWriter csv_writer("state.csv");
+		csv_writer.open();
+		
+		for(auto& r:rows){
+			csv_writer.write_row(r);
+		}
+
+    }
+    catch(std::exception& e){
+        std::cout<<e.what()<<std::endl;
+    }
+    catch(...){
+
+        std::cout<<"Unknown exception occured"<<std::endl;
+    }
+
+   return 0;
+}