Re-add imu_filter_madgwick.

This reverts commit f19cb2f190fc1b98d3510d77534dca716c1ae28a.