├── .gitignore ├── .vscode ├── c_cpp_properties.json └── settings.json ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include ├── InEKF.h ├── LieGroup.h ├── NoiseParams.h └── RobotState.h ├── inekfConfig.cmake.in ├── matlab_example └── README.md └── src ├── InEKF.cpp ├── LieGroup.cpp ├── NoiseParams.cpp ├── RobotState.cpp ├── data ├── correction_speed_test_data.txt ├── imu_kinematic_measurements.txt ├── imu_landmark_kinematic_measurements.txt ├── imu_landmark_measurements.txt └── propagation_speed_test_data.txt ├── examples ├── kinematics.cpp └── landmarks.cpp ├── examples_matlab ├── GenIMUFromTraj.m ├── GenTrajFromIMU.m ├── InEKF │ ├── RIEKF.m │ ├── RIEKF_InitFcn.m │ ├── RIEKF_test.slx │ ├── RIEKF_test.slx.autosave │ ├── Rotation_to_Euler.m │ └── plot_results.m ├── data │ ├── ground_truth │ │ ├── orientation.mat │ │ ├── position.mat │ │ └── velocity.mat │ └── measurements │ │ ├── X_init.mat │ │ ├── angular_velocity.mat │ │ ├── contact.mat │ │ ├── encoders.mat │ │ └── linear_acceleration.mat ├── forward_kinematics │ ├── linux │ │ ├── J_VectorNav_to_LeftToeBottom.mexa64 │ │ ├── J_VectorNav_to_RightToeBottom.mexa64 │ │ ├── R_VectorNav_to_LeftToeBottom.mexa64 │ │ ├── R_VectorNav_to_RightToeBottom.mexa64 │ │ ├── p_VectorNav_to_LeftToeBottom.mexa64 │ │ └── p_VectorNav_to_RightToeBottom.mexa64 │ └── windows │ │ ├── J_VectorNav_to_LeftToeBottom.mexw64 │ │ ├── J_VectorNav_to_RightToeBottom.mexw64 │ │ ├── R_VectorNav_to_LeftToeBottom.mexw64 │ │ ├── R_VectorNav_to_RightToeBottom.mexw64 │ │ ├── p_VectorNav_to_LeftToeBottom.mexw64 │ │ └── p_VectorNav_to_RightToeBottom.mexw64 ├── gen_data_file.m ├── imu_kinematic_measurements.txt ├── imu_landmark_kinematic_measurements.txt ├── imu_landmark_measurements.txt └── run_RIEKF_test.m └── tests ├── correction_speed.cpp └── propagation_speed.cpp /.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/.gitignore -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/.vscode/c_cpp_properties.json -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/.vscode/settings.json -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/CMakeLists.txt -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/LICENSE -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/README.md -------------------------------------------------------------------------------- /include/InEKF.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/include/InEKF.h -------------------------------------------------------------------------------- /include/LieGroup.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/include/LieGroup.h -------------------------------------------------------------------------------- /include/NoiseParams.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/include/NoiseParams.h -------------------------------------------------------------------------------- /include/RobotState.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/include/RobotState.h -------------------------------------------------------------------------------- /inekfConfig.cmake.in: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/inekfConfig.cmake.in -------------------------------------------------------------------------------- /matlab_example/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/matlab_example/README.md -------------------------------------------------------------------------------- /src/InEKF.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/InEKF.cpp -------------------------------------------------------------------------------- /src/LieGroup.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/LieGroup.cpp -------------------------------------------------------------------------------- /src/NoiseParams.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/NoiseParams.cpp -------------------------------------------------------------------------------- /src/RobotState.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/RobotState.cpp -------------------------------------------------------------------------------- /src/data/correction_speed_test_data.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/data/correction_speed_test_data.txt -------------------------------------------------------------------------------- /src/data/imu_kinematic_measurements.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/data/imu_kinematic_measurements.txt -------------------------------------------------------------------------------- /src/data/imu_landmark_kinematic_measurements.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/data/imu_landmark_kinematic_measurements.txt -------------------------------------------------------------------------------- /src/data/imu_landmark_measurements.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/data/imu_landmark_measurements.txt -------------------------------------------------------------------------------- /src/data/propagation_speed_test_data.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/data/propagation_speed_test_data.txt -------------------------------------------------------------------------------- /src/examples/kinematics.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples/kinematics.cpp -------------------------------------------------------------------------------- /src/examples/landmarks.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples/landmarks.cpp -------------------------------------------------------------------------------- /src/examples_matlab/GenIMUFromTraj.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/GenIMUFromTraj.m -------------------------------------------------------------------------------- /src/examples_matlab/GenTrajFromIMU.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/GenTrajFromIMU.m -------------------------------------------------------------------------------- /src/examples_matlab/InEKF/RIEKF.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/InEKF/RIEKF.m -------------------------------------------------------------------------------- /src/examples_matlab/InEKF/RIEKF_InitFcn.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/InEKF/RIEKF_InitFcn.m -------------------------------------------------------------------------------- /src/examples_matlab/InEKF/RIEKF_test.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/InEKF/RIEKF_test.slx -------------------------------------------------------------------------------- /src/examples_matlab/InEKF/RIEKF_test.slx.autosave: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/InEKF/RIEKF_test.slx.autosave -------------------------------------------------------------------------------- /src/examples_matlab/InEKF/Rotation_to_Euler.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/InEKF/Rotation_to_Euler.m -------------------------------------------------------------------------------- /src/examples_matlab/InEKF/plot_results.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/InEKF/plot_results.m -------------------------------------------------------------------------------- /src/examples_matlab/data/ground_truth/orientation.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/data/ground_truth/orientation.mat -------------------------------------------------------------------------------- /src/examples_matlab/data/ground_truth/position.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/data/ground_truth/position.mat -------------------------------------------------------------------------------- /src/examples_matlab/data/ground_truth/velocity.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/data/ground_truth/velocity.mat -------------------------------------------------------------------------------- /src/examples_matlab/data/measurements/X_init.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/data/measurements/X_init.mat -------------------------------------------------------------------------------- /src/examples_matlab/data/measurements/angular_velocity.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/data/measurements/angular_velocity.mat -------------------------------------------------------------------------------- /src/examples_matlab/data/measurements/contact.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/data/measurements/contact.mat -------------------------------------------------------------------------------- /src/examples_matlab/data/measurements/encoders.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/data/measurements/encoders.mat -------------------------------------------------------------------------------- /src/examples_matlab/data/measurements/linear_acceleration.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/data/measurements/linear_acceleration.mat -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/linux/J_VectorNav_to_LeftToeBottom.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/linux/J_VectorNav_to_LeftToeBottom.mexa64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/linux/J_VectorNav_to_RightToeBottom.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/linux/J_VectorNav_to_RightToeBottom.mexa64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/linux/R_VectorNav_to_LeftToeBottom.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/linux/R_VectorNav_to_LeftToeBottom.mexa64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/linux/R_VectorNav_to_RightToeBottom.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/linux/R_VectorNav_to_RightToeBottom.mexa64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/linux/p_VectorNav_to_LeftToeBottom.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/linux/p_VectorNav_to_LeftToeBottom.mexa64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/linux/p_VectorNav_to_RightToeBottom.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/linux/p_VectorNav_to_RightToeBottom.mexa64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/windows/J_VectorNav_to_LeftToeBottom.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/windows/J_VectorNav_to_LeftToeBottom.mexw64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/windows/J_VectorNav_to_RightToeBottom.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/windows/J_VectorNav_to_RightToeBottom.mexw64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/windows/R_VectorNav_to_LeftToeBottom.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/windows/R_VectorNav_to_LeftToeBottom.mexw64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/windows/R_VectorNav_to_RightToeBottom.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/windows/R_VectorNav_to_RightToeBottom.mexw64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/windows/p_VectorNav_to_LeftToeBottom.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/windows/p_VectorNav_to_LeftToeBottom.mexw64 -------------------------------------------------------------------------------- /src/examples_matlab/forward_kinematics/windows/p_VectorNav_to_RightToeBottom.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/forward_kinematics/windows/p_VectorNav_to_RightToeBottom.mexw64 -------------------------------------------------------------------------------- /src/examples_matlab/gen_data_file.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/gen_data_file.m -------------------------------------------------------------------------------- /src/examples_matlab/imu_kinematic_measurements.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/imu_kinematic_measurements.txt -------------------------------------------------------------------------------- /src/examples_matlab/imu_landmark_kinematic_measurements.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/imu_landmark_kinematic_measurements.txt -------------------------------------------------------------------------------- /src/examples_matlab/imu_landmark_measurements.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/imu_landmark_measurements.txt -------------------------------------------------------------------------------- /src/examples_matlab/run_RIEKF_test.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/examples_matlab/run_RIEKF_test.m -------------------------------------------------------------------------------- /src/tests/correction_speed.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/tests/correction_speed.cpp -------------------------------------------------------------------------------- /src/tests/propagation_speed.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RossHartley/invariant-ekf/HEAD/src/tests/propagation_speed.cpp --------------------------------------------------------------------------------