FOR ELEC6910P CLASS
If you are a student in ELEC6910P, you ca take my code as reference.
ekf_node_eulerange is similar to your work, I code it last year, I don't konw whether it work or not.
ekf_node_naive is the quaternion derivative version, works well this year. However, you don't know how de derivate quaternion, so you cann't use it.
ekf_node is the quaternion version with time sychronize. Best performance.
If you totally copy my code, I will know!!!! ^ ^