catching more errors and updated the innovation function with a proper quaternion distance
starting to implement the last steps.
building P matrix
innovation function takes now all measurements and estimation at once and provides on big innovation vector as result
kalman gain
update state estimation
update covariance estimation
plotting included
first (buggy) version implemented
last changes for this week