`>>> Q = np.diag([0.1, 0.1, np.deg2rad(1)]) ** 2;` should be: `>>> R = np.diag([0.1, 0.1, np.deg2rad(1)]) ** 2;`