Updated kalman_convention_fix_node.cpp to debug the jerky behaviour