Mavlink message over serial for EKF position measurement update

Hi,
I’m just starting up with modifying px4 firmware to add an additional three measurements to my EKF measurement model. These measurements are the vehicle’s local position w.r.t some reference point in NED coordinate frame. These measurements are gathered from an onboard camera recognising ArUco codes (like QR codes) placed at known locations around a room. The images are processed and the vehicle’s position w.r.t these tags is estimated.

I’ve packed these measurements into a mavlink message (LOCAL_POSITION_NED (#32)) message and am sending it to my Pixracer via serial connection.

My question is this: how do I subscribe to this message coming in? Do I need to create a custom uOrb message? I’d like to modify the EKF to make use of these three additional position measurements.

Would I add something like orb_subscribe(ORB_ID(custom_orb_message)) in the ekf::ekf constructor and then an orb_check( ) in the ekf::run() function?

A big hurdle for me right now is subscribing to this mavlink message and unpacking it into position measurements.

Any help on this matter would be greatly appreciated.
Thanks guys!

EDIT: I’ve updated the mavlink_receiver.h and mavlink_receiver.cpp to receive mavlink_position_local_ned message type. Can I use the <uORB/topics/vehicle_local_position.h> topic to publish this mavlink message?

Why don’t you send them as vision position messages? Then you can use the already defined vision ekf2 variables to change the weightings.

Local_Position_Ned is the output for the EKF2 not an input. So you should not be sending that message.

Have a look at this cpp repository.

Look at the function Position_Controller::update_current_position that shows you how to make and send the vision position estimate. You can also send covariances with it.
http://mavlink.org/messages/common#VISION_POSITION_ESTIMATE

1 Like

thank you rijesha!

Is there a specific reason why you recommend the vision topic over the LOCAL_POSITION_NED?

I am working on an indoor (onboard) localization system which only provides x,y,z also from an companion computer - how would you handle the rotations? just set everything to zero + high uncertainty in the covariance?

Best
Daniel

As far as I know, LOCAL_POSITION_NED is an output from the EKF2. If you want to use that message as an input then you will need to do a huge reworking of the px4 system.

If you are looking to send highly accurate local position messages then vision position estimate is the best way to do it. You can send local x,y,z and also send roll,pitch, and yaw (if those are available) directly with it.

Then you can use EKF2_AID_MASK to enable vision position and maybe vision yaw.
I don’t think roll and pitch are implemented into the estimator. Rather they are generated using the IMU. If you do not need yaw then in EKF2_AID_MASK disable the vision yaw fusion and try to rely on magnetic fields and gyros to estimate the yaw of the drone.

If you are inside a building you may need to calculate earth’s magnetic field indoors for it.

regards
Rijesh

1 Like

Hi Rjesha,
How can I utilise parameters I have defined in a c file elsewhere? Explicitly, if I create a c file with parameters defined by PARAM_DEFINE_FLOAT(param_name, value) in the EKF2 module folder, how can I reference those parameters in local_position_estimator_main.cpp?
Is there a function to call in the script to access those defined parameters?
Thanks