This module contains a Kalman filter implementation and simulation of the rotation dynamics of solid bodies in 2D & 3D.
This module is written to gain an understanding of how a Kalman filter for attitude estimation works.
There are unit tests for most of the essential components.
They are in the *_test.m
files.
To run the unit tests simply run the matlab command clear classes; runtests
.
For live visualization run the script run_visualization.m
in matlab.
To run the simulation and show a plot afterwards run the script run_graph.m
in matlab. This script does multiple runs (each with random initialization) and shows the estimation error plot for rate and attitude.
math subroutines:
quatconj.m
: conjugate of quaternionquatmult.m
: quaternion multiplicationrotate_by_quaternion.m
: rotate a 3D vector by a quaternionrotation_matrix_from_quat.m
: generate a rotation matrix from a quaternionquat_rand.m
: generate a random quternion with uniform probabilityquat_from_two_vect.m
: compute a quaternion that rotates a vector to another vectorcross_prod_matrix
: build the cross product matrix from a vector
Kalman filter:
ExtendedKalmanFilter.m
: generic EKF implementationKalmanFilter.m
: generic KF implementation (there is no constistent distinction in variable naming between F (continuous time) and Phi (discrete time), this should be changed someday)EKF3DConstMomentum.m
: simple EKF for 3D body assuming constant momentumEKF3DConstMomentumSymbolicDerivation.m
: symbolic derivations for the aboveMEKF3DConstMomentum.m
: Multiplicative EKF with constant momentum modelMEKF3DConstMomentumSymbolicDerivation.m
: symbolic derivations for the aboveMEKF3DGyro.m
: Multiplicative EKF with gyroscope inputMEKF3DGyroSymbolicDerivation.m
: symbolic derivations for the above
Simulation:
RotationBody3D.m
: simulates 3d rotation dynamics & sensor readingsSimulation3DBody.m
: runs the kalman together with 3d body simulation
Plotting:
run_visualization.m
: run the simulation and visualize 3d cubecube_plot.m
: draw a cube, used in run_visualization.mrun_graph.m
: run multiple simulations and plot error graphs
Simple 2d KF example: (this is just a toy example of a linear Kalman filter)
runPosVel2DSystem.m
: set up and run 2d KalmanRotationBody2D.m
: simulates 2d rotation dynamics & sensor readingsplot_KF_results.m
: 2d graphs with error bar