Plane : Altitude Calibration : Added constrain on P matrix diagonals