EKF EV-Height Rotation

[isidroas]

Hi all, i have been working on a Aruco based quadrotor witch has an intermittent source of height. I found that it should be rotated in the same way as the position is rotated by _R_ev_to_ekf.

I think that this line,

PX4-Autopilot/src/modules/ekf2/EKF/height_fusion.cpp

Line 110 in 43a04c9

_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);

could be changed by:

		       Vector3f ev_pos_meas = _ev_sample_delayed.pos;
             
			if (_params.fusion_mode & MASK_ROTATE_EV) {
                                calcExtVisRotMat();
				ev_pos_meas = _R_ev_to_ekf * ev_pos_meas;
			}
                        _ev_pos_innov(2) = _state.pos(2) - ev_pos_meas(2);

also the Ekf::resetHeight() function would need to be changed