Kalman Filter

A 2D kalman filter tutorial for self_t+dt and any explorer from space time.

To design a kalman filter for an object moving on a 2D plane with 3 degrees of freedom (traslation in x, traslation in y and rotation along z or yaw).

Lite version

No jupyter notebook outputs at: https://github.com/MRo47/KalmanFilter/tree/lite

Tutorial

find tutorial at https://mro47.github.io/portfolio/work-kalman-filter.html

Kalman filter tracking robot position by fusing imu and position measurements

Dependencies

For modules
1. numpy
2. matplotlib
3. scipy (misc.derivative)
For notebooks and animations
1. ffmpeg
2. jupyter nb

Run

Run main file

python3 main.py

Edit these lines to play or save animations default=play

anim.run()
# anim.run(save_path='images/KF_constant_velocity_anim.gif')
anim.error_analysis()
# anim.error_analysis(save_path='images/KF_constant_velocity_err.png')

Or play with the Jupyter notebook

Things to play with

Change functions in modules/data_gen.py under functions x and y to generate different paths for the robot.

Coefficients can be changed or th function can be replaced fully just make sure the x y limits are set properly in the plots later in main.py otherwise the graphs would be out of bounds.

#pass limits here check the Animator class in modules/visual.py
anim = Animator('Robot position (Kalman Filter with sensor fusion gps + imu)', plt, total_iters, animation_interval_ms,ideal_data_f, noisy_data_f, kf, Animator.FilterType.SF,start_time=time_lims[0])

Change noise in the acceleration and position data from main.py at

# standard deviation of gaussian noise to be added in position
 pos_dev = [2.2, 2.2, 0.05]
 # standard deviation of gaussian noise to be added in acceleration
 acc_dev = [0.1, 0.1, 0.01]

Change missing timestamps in main.py at

# timestamps where data is not available (np.nan) from sensors
 missing_pos_data = (10, 11, 12, 30, 31, 32, 33, 34, 50, 51, 52)
 # acceleration is not used as measurement (constant velocity,acc=0)
 missing_accel_data = ()

Change and analyse how initial and system noise covariances affect the output

# initial noise estimate in state (position, velocity)
 p_diag = np.matrix([100, 100, 100, 600, 600, 600, 1000, 1000, 1000])
 # noise in acceleration
 q_diag = np.matrix(1e-2 * np.ones(3))

MRo47 / KalmanFilter