ekf2: EV overhaul yaw and position fusion

- new EV position offset estimator
 - yaw_align now strictly means north (no more rotate external vision aid mask)
 - automatically switching between EV yaw, and yaw align north based on
GPS quality

msg/CMakeLists.txt

@@ -74,6 +74,7 @@ set(msg_files
 	EstimatorAidSource2d.msg
 	EstimatorAidSource3d.msg
 	EstimatorBias.msg
+	EstimatorBias3d.msg
 	EstimatorEventFlags.msg
 	EstimatorGpsStatus.msg
 	EstimatorInnovations.msg

msg/EstimatorBias.msg

@@ -9,4 +9,4 @@ float32 innov			# innovation of the last measurement fusion (m)
 float32 innov_var		# innovation variance of the last measurement fusion (m^2)
 float32 innov_test_ratio	# normalized innovation squared test ratio
 
-# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias estimator_ev_hgt_bias
+# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias

msg/EstimatorBias3d.msg

@@ -0,0 +1,14 @@
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+
+uint32 device_id                # unique device ID for the sensor that does not change between power cycles
+
+float32[3] bias                 # estimated barometric altitude bias (m)
+float32[3] bias_var             # estimated barometric altitude bias variance (m^2)
+
+float32[3] innov                # innovation of the last measurement fusion (m)
+float32[3] innov_var            # innovation variance of the last measurement fusion (m^2)
+float32[3] innov_test_ratio     # normalized innovation squared test ratio
+
+# TOPICS estimator_bias3d
+# TOPICS estimator_ev_pos_bias

msg/VehicleOdometry.msg

@@ -28,4 +28,4 @@ uint8 reset_counter
 int8 quality
 
 # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
-# TOPICS estimator_odometry estimator_visual_odometry_aligned
+# TOPICS estimator_odometry