**To test your MM with the vehicle please follow the instructions as per http://ardupilot.org/copter/docs/common-pozyx.html **
But for the parameters, set
BCN_TYPE=2
Use EKF3 to test your code
Set the frequency of injection in your MM dashboard to 4Hz.
This blogpost contains the information that was exchanged between me, @rmackay9, @priseborough, @MHefny, and couple others who were testing the Marvelmind beacons (http://www.marvelmind.com)(henceforth referred to as MM).
At the time of writing this blog, MM was not injecting the distance between each stationary beacons and the mobile beacon. This led us to manually calculate the distance between each stationary beacon and the mobile beacon and inject this into the EKF. But due to other issues, the EKF wasn’t too happy and there was some conflictions which made the vehicle jump all over the map even though the vehicle was stationary throughout.
This post contains some the ideas that were exchanged and tested!
- The vehicle does not receive a lock when the rate of injection of MM data is >8 Hz. To get optimal results a injection rate of 4 Hz was most optimal.
Thanks to @MHefny for the videos and analysis
This is a Comparision between 4Hz & 12 Hz update while Beacons are all left intact even the Quad. so all data should be fixed.
- Define AP’s own virtual beacons in MM beacon code, and then use the distance calculation approach that is done in AP MM driver to calculate distance with these virtual beacons,ignoring the real beacons distance and rely only on the given XYZ of the vehicle.
This could be a generic driver for all types of beacons that has accurate XYZ like MM, and still does not corrupt EKF code.
This is a video for hardcoded values:
@priseborough’s reply to this,
It is fusing the beacons, however there is a 0.8m range innovation on the nearest beacon data which is ID 0. This is possibly caused by the EKF trying to resolve a height error because beacon ID is only 1.3m range away, so the effect of a height offset will be increased due to the increased elevation angle relative to the beacon.
Other than that it appears to be successfully fusion beacon data.
There is something else going on with the data that is causing measurements from different range beacons to be not fused for significant periods, which is causing a lot of the horizontal position movement. This is evidenced by the position states NKF1.PN and NKF1.PE moving whilst the range innovations NKF0.innov are flat-lining:
Range innovations
Range measurements
Position states
At first i thought it was the innovation checks failing, but that is not the case. When measurements are fused, the variation in innovations is less than ±0.1m plus the offset of 0.6m in the innovation for beacon ID 0. The innovation test ratios are small when they are being updated.
Failure to fuse data can be caused by stamping or timing jitter putting the measurements outside the window they need to be for them to be retrieved by the buffer. Diagnosing this is going to require additional data logging or interactive debugging with a probe.
BTW, baro is already being used for height because EK2_ALT_SOURCE is set to 0. To use beacons for height, the beacons would need to be placed in a configuration that was not co-planar to resolve the height ambiguity created by having them all at the same or similar heights, and EK3_ALT_SOURCE should be set to 3.
The fusion of range beacon data was originally developed to work with the Pozyx system. That system read the RF beacons sequentially and did not provide a reliable xyz measurement in flight. Fusng the raw measurements gave a superior result and also enabled temporary dropouts to be handled better. To use the xyz directly, the logical approach would be to convert it to lat/lon/alt and consume it as GPS data
Data file 5 Data5_Bad.bin (175.8 KB)
shows that timing jitter meant that the EKF3 was unable to process data for beacon ID’s 1 and 2. I think the processor is being asked to do too much (it is processing 3 EKF instances) so I recommend that while we are doing the debugging EK2_ENABLE be set to 0 and that EK3_IMU_MASK be set to 1 so we are using a single EKF and IMU. We are also missing pre-arm data. Can you please set LOG_DISARMED to 1 for future testing.
Data file 1 Data1.bin (178.7 KB)
shows that the initial EKF position alignment checks did not pass. I will add a debug message to record all the metrics used by the alignment check.
I notice that the beacon system is reporting a zero height - this is an important detail because the EKF assumes it is operating in a 3D mode when it uses the data in the alignment checks. That means we will have modify the interface to indicate is the beacon systems position estimate is 2D or 3D.
Paulo from info@airborneprojects.com wrote:
I attach the results on our tests with the Marvelmind beacons as per our engineers. We have 2 different attempts, both on V3.5.0-RC7.
- On our first attempts we were not even able to have the Pixhawk started if the MarvelMind was already initialized. We overcame this by having it powered though the Piwhawk BEC, so that by the time the MarvelMind was initialized the Pixhawk was also.
- On the second attempts we are having similar results to the ones that Mohammad showed on the video, even though for some reason we never had troubles with the orientation of the movement.
We saw that there are many changes in the EKF for the RC8 and we will have a try at it. We will also make some tweaks on the EKF parameters to see if there is any improvement.
1st Report from 14/06/2017
On the previous tries to connect marvelmind system to pixhawk, the MM beacon was already powered on when i turned on pixhawk.
Now, I tried powering both at the same time, did this using a BEC to power the beacon instead of using it’s own battery like before.
First, I turn on the MM system with the stationary beacons and generate and freeze the map.
Then i connect both the PH and MM beacon. PH boots like normal and shows coordinates 0,0. The dashboard starts looking for mobile beacon (sometimes finds it fast, others I have to put the beacon on sleeping mode and wake it up again, i believe this has to do with beacon power saving configs).
When the MM modem finds the beacon I can see it on dashboard map (some points can be wrong on the first 2-3 seconds).
Pixhawk updates coordinates about 30 seconds after that and I can see the quad on mission planner map.
I did some tests to see the precision of the location on mission planner with different Marvemind “location update rate” 4, 8, 12, 16 and 16+.
I did the tests on a ~1.8m side square on the ground, i start with the quad on the center on the square, do the above steps until i get a position on mission planner map and then move the quad in the following order:
center - left - center - right - center - up - center - down - center
where up is north and right is east of the dashboard map
I kept the quad always facing north (up)
The PH parameters where the same as before except for BCN_ORIENT_YAW = -18 I tried to measure it with tha valued on PH when pointing north of the beacons but as i am inside a lab it is not constant in time nor on all parts of the square so i eyeballed an average value.
Beacon “protocol on UART/USB output” was “marvelmind” mode
Attached are the logs and pictures of the marvelmind dashboard and missionplanner
log 2 - 4Hz
log 2 - 8Hz
log 2 - 12Hz
log 2 - 16Hz
log 2 - 16+Hz
---------------------------------------//----------------------------------------------
2nd Report 13/06/2017
This are my attempts on connecting marvelmind system to pixhawk.
I enabled LOG_DISARMED and made 7 logs to compare the results
On log 1, marvelmind beacon is not connected to pixhawk and i could arm the copter
then i connected the beacon with GND and TX to serial 1 (Telem 1 port) on pixhawk, i tried the same with beacon RX connected also. I could not arm the pixhawk on any of the next 6 tries.
log 2 - beacon on “marvelmind” mode and RX connected
log 3 - beacon on “testing data” mode and RX connected
log 4 - beacon on “testing data” mode and RX not connected
log 5 - beacon on “marvelmind” mode and RX not connected
log 6 - beacon on “NMEA” mode and RX not connected
log 7 - beacon on “NMEA” mode and RX connected
The beacon mode refers to the marvelmind dashboard parameter “protocol on UART/USB output”
I am using APM:Copter V.3.5.0-rc7 and these are my parameters:
BCN_ALT 654
BCN_LATITUDE 40,27811
BCN_LONGITUDE -7,513942
BCN_ORIENT_YAW 348 (measured value on pixhawk while pointing north of the beacons)
BCN_TYPE 2
EK2_BCN_DELAY 50
EK2_BCN_I_GTE 500
EK2_BCN_M_NSE 1
BRD_SER1_RTSCTS 0
SERIAL1_BAUD 115
SERIAL1_PROTOCOL 13
GPS_TYPE 0
GPS_TYPE2 0
ARMING_CHECK -14
When connected to copter i can see some errors on mission planner like:
-Bad compass health
-Error pos horiz variance
-gyros still settling
The ground speed keeps increasing
On the EKF window i can also see Position (Horiz) incresing with time (pics attached)
Sometimes when i connect the battery the pixhawk makes a tone different than the usual
@priseborough
I have tracked down bugs that were causing problems with the baro height fighting the beacon constellation height. This resulted in poor height and position stability when there was baro drift. I have verified these changes in SITL, but they still need to be hardware tested.
As you can see this is an ongoing investigation, updates will be posted here.