diff --git a/README.md b/README.md
index 22e9646..cd7b010 100644
--- a/README.md
+++ b/README.md
@@ -34,13 +34,14 @@ The following is an indicative list of examples.
- REINFORCE algorithm on ```CartPole-v0```
- Expected SARSA on ```CliffWalking-v0```
- Approximate Monte Carlo on ```MountainCar-v0```
-- Monte Carlo tree search on ```Taxi-v3```
+- Monte Carlo tree search on ```Taxi-v3```
- Double Q-learning on ```CartPole-v0```
- First visit Monte Carlo on ```FrozenLake-v0```
### Filtering
- Kalman filtering
+- Extended Kalman filter for diff-drive system
### Path planning
diff --git a/examples/filtering/CMakeLists.txt b/examples/filtering/CMakeLists.txt
index 07a51d4..130e5d0 100644
--- a/examples/filtering/CMakeLists.txt
+++ b/examples/filtering/CMakeLists.txt
@@ -1,2 +1,3 @@
ADD_SUBDIRECTORY(filtering_example_1)
+ADD_SUBDIRECTORY(filtering_example_2)
diff --git a/examples/filtering/filtering_example_2/CMakeLists.txt b/examples/filtering/filtering_example_2/CMakeLists.txt
new file mode 100644
index 0000000..c09580f
--- /dev/null
+++ b/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)
diff --git a/examples/filtering/filtering_example_2/filtering_example_2.cpp b/examples/filtering/filtering_example_2/filtering_example_2.cpp
new file mode 100755
index 0000000..eee917d
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#include
+#include
+
+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 input_type;
+
+ ObservationModel();
+
+ // simply return the state
+ const DynVec evaluate(const DynVec& input)const;
+
+ // get the H or M matrix
+ const DynMat& get_matrix(const std::string& name)const;
+
+private:
+
+ DynMat H;
+ DynMat M;
+};
+
+ObservationModel::ObservationModel()
+ :
+ H(2, 3),
+ M(2, 2)
+{
+ H = DynMat::Zero(2,3);
+ M = DynMat::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
+ObservationModel::evaluate(const DynVec& input)const{
+ return input;
+}
+
+const DynMat&
+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 get_measurement(const SysState<3>& state){
+
+ DynVec 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 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 input;
+ input["v"] = 1.0;
+ input["w"] = 0.0;
+ input["errors"] = motion_control_error;
+ motion_model.initialize_matrices(input);
+
+ ObservationModel observation;
+
+ ExtendedKalmanFilter ekf(motion_model, observation);
+
+ DynMat R = DynMat::Zero(2, 2);
+ R(0,0) = 1.0;
+ R(1, 1) = 1.0;
+
+ DynMat Q = DynMat::Zero(2, 2);
+ Q(0,0) = 0.001;
+ Q(1, 1) = 0.001;
+
+ DynMat P = DynMat::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 names{"Time", "X_true", "Y_true", "Theta_true", "X", "Y", "Theta"};
+ writer.write_column_names(names);
+
+ try{
+
+
+ BOOST_LOG_TRIVIAL(info)<<"Starting simulation... "<