diff --git a/CMakeLists.txt b/CMakeLists.txt
index c5c0a54..a7f6ac9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,20 +1,14 @@
 # Specify the minimum version for CMake
 cmake_minimum_required(VERSION 2.8.3)
-# set(CMAKE_CXX_STANDARD 11)
-#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") # Debugging
-#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") 
+# Project's name
+project(inekf)
+
+# Set compiler flags
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -DEIGEN_NO_DEBUG -march=native -Wl,--no-as-needed") 
 SET(CMAKE_CXX_COMPILER /usr/bin/g++)
-# -O3 -DEIGEN_NO_DEBUG sped up large matrix multiplication by a factor of 10
-# -march=native further sped up large matrix multiplication by a factor of 2
 
-# Project's name
-project(inekf)
 
-# Set the output folder where your program will be created
-#set(CMAKE_BINARY_DIR ${CMAKE_SOURCE_DIR}/bin)
-#set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR})
-#set(LIBRARY_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/lib)    
+# Set the output folder where your program will be created  
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
 set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib)
 
diff --git a/matlab_example/GenIMUFromTraj.m b/src/examples_matlab/GenIMUFromTraj.m
similarity index 100%
rename from matlab_example/GenIMUFromTraj.m
rename to src/examples_matlab/GenIMUFromTraj.m
diff --git a/matlab_example/GenTrajFromIMU.m b/src/examples_matlab/GenTrajFromIMU.m
similarity index 100%
rename from matlab_example/GenTrajFromIMU.m
rename to src/examples_matlab/GenTrajFromIMU.m
diff --git a/matlab_example/example_code/RIEKF.m b/src/examples_matlab/InEKF/RIEKF.m
similarity index 100%
rename from matlab_example/example_code/RIEKF.m
rename to src/examples_matlab/InEKF/RIEKF.m
diff --git a/matlab_example/example_code/RIEKF_InitFcn.m b/src/examples_matlab/InEKF/RIEKF_InitFcn.m
similarity index 100%
rename from matlab_example/example_code/RIEKF_InitFcn.m
rename to src/examples_matlab/InEKF/RIEKF_InitFcn.m
diff --git a/matlab_example/example_code/RIEKF_test.slx b/src/examples_matlab/InEKF/RIEKF_test.slx
similarity index 100%
rename from matlab_example/example_code/RIEKF_test.slx
rename to src/examples_matlab/InEKF/RIEKF_test.slx
diff --git a/matlab_example/example_code/RIEKF_test.slx.autosave b/src/examples_matlab/InEKF/RIEKF_test.slx.autosave
similarity index 100%
rename from matlab_example/example_code/RIEKF_test.slx.autosave
rename to src/examples_matlab/InEKF/RIEKF_test.slx.autosave
diff --git a/matlab_example/example_code/Rotation_to_Euler.m b/src/examples_matlab/InEKF/Rotation_to_Euler.m
similarity index 100%
rename from matlab_example/example_code/Rotation_to_Euler.m
rename to src/examples_matlab/InEKF/Rotation_to_Euler.m
diff --git a/matlab_example/example_code/plot_results.m b/src/examples_matlab/InEKF/plot_results.m
similarity index 100%
rename from matlab_example/example_code/plot_results.m
rename to src/examples_matlab/InEKF/plot_results.m
diff --git a/matlab_example/data/ground_truth/orientation.mat b/src/examples_matlab/data/ground_truth/orientation.mat
similarity index 100%
rename from matlab_example/data/ground_truth/orientation.mat
rename to src/examples_matlab/data/ground_truth/orientation.mat
diff --git a/matlab_example/data/ground_truth/position.mat b/src/examples_matlab/data/ground_truth/position.mat
similarity index 100%
rename from matlab_example/data/ground_truth/position.mat
rename to src/examples_matlab/data/ground_truth/position.mat
diff --git a/matlab_example/data/ground_truth/velocity.mat b/src/examples_matlab/data/ground_truth/velocity.mat
similarity index 100%
rename from matlab_example/data/ground_truth/velocity.mat
rename to src/examples_matlab/data/ground_truth/velocity.mat
diff --git a/matlab_example/data/measurements/X_init.mat b/src/examples_matlab/data/measurements/X_init.mat
similarity index 100%
rename from matlab_example/data/measurements/X_init.mat
rename to src/examples_matlab/data/measurements/X_init.mat
diff --git a/matlab_example/data/measurements/angular_velocity.mat b/src/examples_matlab/data/measurements/angular_velocity.mat
similarity index 100%
rename from matlab_example/data/measurements/angular_velocity.mat
rename to src/examples_matlab/data/measurements/angular_velocity.mat
diff --git a/matlab_example/data/measurements/contact.mat b/src/examples_matlab/data/measurements/contact.mat
similarity index 100%
rename from matlab_example/data/measurements/contact.mat
rename to src/examples_matlab/data/measurements/contact.mat
diff --git a/matlab_example/data/measurements/encoders.mat b/src/examples_matlab/data/measurements/encoders.mat
similarity index 100%
rename from matlab_example/data/measurements/encoders.mat
rename to src/examples_matlab/data/measurements/encoders.mat
diff --git a/matlab_example/data/measurements/linear_acceleration.mat b/src/examples_matlab/data/measurements/linear_acceleration.mat
similarity index 100%
rename from matlab_example/data/measurements/linear_acceleration.mat
rename to src/examples_matlab/data/measurements/linear_acceleration.mat
diff --git a/matlab_example/forward_kinematics/linux/J_VectorNav_to_LeftToeBottom.mexa64 b/src/examples_matlab/forward_kinematics/linux/J_VectorNav_to_LeftToeBottom.mexa64
similarity index 100%
rename from matlab_example/forward_kinematics/linux/J_VectorNav_to_LeftToeBottom.mexa64
rename to src/examples_matlab/forward_kinematics/linux/J_VectorNav_to_LeftToeBottom.mexa64
diff --git a/matlab_example/forward_kinematics/linux/J_VectorNav_to_RightToeBottom.mexa64 b/src/examples_matlab/forward_kinematics/linux/J_VectorNav_to_RightToeBottom.mexa64
similarity index 100%
rename from matlab_example/forward_kinematics/linux/J_VectorNav_to_RightToeBottom.mexa64
rename to src/examples_matlab/forward_kinematics/linux/J_VectorNav_to_RightToeBottom.mexa64
diff --git a/matlab_example/forward_kinematics/linux/R_VectorNav_to_LeftToeBottom.mexa64 b/src/examples_matlab/forward_kinematics/linux/R_VectorNav_to_LeftToeBottom.mexa64
similarity index 100%
rename from matlab_example/forward_kinematics/linux/R_VectorNav_to_LeftToeBottom.mexa64
rename to src/examples_matlab/forward_kinematics/linux/R_VectorNav_to_LeftToeBottom.mexa64
diff --git a/matlab_example/forward_kinematics/linux/R_VectorNav_to_RightToeBottom.mexa64 b/src/examples_matlab/forward_kinematics/linux/R_VectorNav_to_RightToeBottom.mexa64
similarity index 100%
rename from matlab_example/forward_kinematics/linux/R_VectorNav_to_RightToeBottom.mexa64
rename to src/examples_matlab/forward_kinematics/linux/R_VectorNav_to_RightToeBottom.mexa64
diff --git a/matlab_example/forward_kinematics/linux/p_VectorNav_to_LeftToeBottom.mexa64 b/src/examples_matlab/forward_kinematics/linux/p_VectorNav_to_LeftToeBottom.mexa64
similarity index 100%
rename from matlab_example/forward_kinematics/linux/p_VectorNav_to_LeftToeBottom.mexa64
rename to src/examples_matlab/forward_kinematics/linux/p_VectorNav_to_LeftToeBottom.mexa64
diff --git a/matlab_example/forward_kinematics/linux/p_VectorNav_to_RightToeBottom.mexa64 b/src/examples_matlab/forward_kinematics/linux/p_VectorNav_to_RightToeBottom.mexa64
similarity index 100%
rename from matlab_example/forward_kinematics/linux/p_VectorNav_to_RightToeBottom.mexa64
rename to src/examples_matlab/forward_kinematics/linux/p_VectorNav_to_RightToeBottom.mexa64
diff --git a/matlab_example/forward_kinematics/windows/J_VectorNav_to_LeftToeBottom.mexw64 b/src/examples_matlab/forward_kinematics/windows/J_VectorNav_to_LeftToeBottom.mexw64
similarity index 100%
rename from matlab_example/forward_kinematics/windows/J_VectorNav_to_LeftToeBottom.mexw64
rename to src/examples_matlab/forward_kinematics/windows/J_VectorNav_to_LeftToeBottom.mexw64
diff --git a/matlab_example/forward_kinematics/windows/J_VectorNav_to_RightToeBottom.mexw64 b/src/examples_matlab/forward_kinematics/windows/J_VectorNav_to_RightToeBottom.mexw64
similarity index 100%
rename from matlab_example/forward_kinematics/windows/J_VectorNav_to_RightToeBottom.mexw64
rename to src/examples_matlab/forward_kinematics/windows/J_VectorNav_to_RightToeBottom.mexw64
diff --git a/matlab_example/forward_kinematics/windows/R_VectorNav_to_LeftToeBottom.mexw64 b/src/examples_matlab/forward_kinematics/windows/R_VectorNav_to_LeftToeBottom.mexw64
similarity index 100%
rename from matlab_example/forward_kinematics/windows/R_VectorNav_to_LeftToeBottom.mexw64
rename to src/examples_matlab/forward_kinematics/windows/R_VectorNav_to_LeftToeBottom.mexw64
diff --git a/matlab_example/forward_kinematics/windows/R_VectorNav_to_RightToeBottom.mexw64 b/src/examples_matlab/forward_kinematics/windows/R_VectorNav_to_RightToeBottom.mexw64
similarity index 100%
rename from matlab_example/forward_kinematics/windows/R_VectorNav_to_RightToeBottom.mexw64
rename to src/examples_matlab/forward_kinematics/windows/R_VectorNav_to_RightToeBottom.mexw64
diff --git a/matlab_example/forward_kinematics/windows/p_VectorNav_to_LeftToeBottom.mexw64 b/src/examples_matlab/forward_kinematics/windows/p_VectorNav_to_LeftToeBottom.mexw64
similarity index 100%
rename from matlab_example/forward_kinematics/windows/p_VectorNav_to_LeftToeBottom.mexw64
rename to src/examples_matlab/forward_kinematics/windows/p_VectorNav_to_LeftToeBottom.mexw64
diff --git a/matlab_example/forward_kinematics/windows/p_VectorNav_to_RightToeBottom.mexw64 b/src/examples_matlab/forward_kinematics/windows/p_VectorNav_to_RightToeBottom.mexw64
similarity index 100%
rename from matlab_example/forward_kinematics/windows/p_VectorNav_to_RightToeBottom.mexw64
rename to src/examples_matlab/forward_kinematics/windows/p_VectorNav_to_RightToeBottom.mexw64
diff --git a/matlab_example/gen_data_file.m b/src/examples_matlab/gen_data_file.m
similarity index 100%
rename from matlab_example/gen_data_file.m
rename to src/examples_matlab/gen_data_file.m
diff --git a/matlab_example/imu_kinematic_measurements.txt b/src/examples_matlab/imu_kinematic_measurements.txt
similarity index 100%
rename from matlab_example/imu_kinematic_measurements.txt
rename to src/examples_matlab/imu_kinematic_measurements.txt
diff --git a/matlab_example/imu_landmark_measurements.txt b/src/examples_matlab/imu_landmark_measurements.txt
similarity index 100%
rename from matlab_example/imu_landmark_measurements.txt
rename to src/examples_matlab/imu_landmark_measurements.txt
diff --git a/matlab_example/run_RIEKF_test.m b/src/examples_matlab/run_RIEKF_test.m
similarity index 85%
rename from matlab_example/run_RIEKF_test.m
rename to src/examples_matlab/run_RIEKF_test.m
index 7e8519a..252f648 100644
--- a/matlab_example/run_RIEKF_test.m
+++ b/src/examples_matlab/run_RIEKF_test.m
@@ -5,7 +5,7 @@
 
 % Add paths
 addpath(genpath('forward_kinematics'))
-addpath('example_code')
+addpath('InEKF')
 
 %% Open Model  
 mdl = 'RIEKF_test';