East West Oscillation in VelocityRoam, Only with Authentic DJI GPS
Description
Environment
Slow 500 quad with 16.09, AutoTuned PIDs, DJI (authentic, does not happen with clone) GPS.
Activity
Rotate yaw 180 degrees when hovering in VelocityRoam and it (one test flight) moves position about 2 meters or so (very probably unrelated to these code changes). Rotate another 180 and it is back to where it was. This implies strongly that the returned GPS coordinates are different when rotated in yaw axis at a fixed location. The GPS does not handle yaw rotation without seeing a position change.
What about other axes (roll and pitch)? Do they also cause a different position? Is the reported GPS velocity also affected?
Is it possible to code an automatically tuned offset ellipsoid such that it knows the offsets of velocity and position for every angle? After any "quick" 3D angle change it would know "more" about "a range" of angles. There are some symmetries to help this, Rotating yaw 180 would give info about the full 360. Perhaps each angle change is an ellipse fitting with the ellipse centered at the origin.
Now, about some code I wrote to fix the DJI issue: Assuming the DJI filtering can be modelled as an integration I developed some code that adds some derivative, and it works. It needs more tuning, and I find that the old OP fast oscillation that is sensitive to compass heading pops up when I tune it up tightly enough to remove most of the slow oscillation.
I believe that the (old OP) fast oscillation is caused by GPS angle change -> false velocity change detected -> commanded bank angle change -> (repeat)
I suspect that the fast oscillation can be tuned out by reducing the velocity PI, or whichever PI it is that has units of degrees per meters per second. I will tune the code and tune the PI and see what I can get out of what I have so far.
Update 2017-01-30:
I have proven that this is caused by the excessive data filtering in the authentic DJI GPS. The same thing happens if you take quad with GPSv9 or other Ublox GPS, that flies well in VelocityRoam, and configure the GPSv9 to a highly filtered GPSSettings -> UbxDynamicModel (DynModel in Ublox terms) such as Pedestrian. It seems likely that DJI uses the "Automotive" setting internally (then perhaps adds more filtering with code, or "Automotive" in the old Ublox 6 GPS in the DJI GPS is worse than newer UBlox GPSs) with the Ublox 6 that the DJI is made from since DJI claims 6000m max altitude and that is the exact number Ublox quotes for Automotive.
Ublox dynModel Velocity m/s Vertical Velocity m/s Altitude m Position Deviation
PORTABLE = 0 310 50 12000 Medium
STATIONARY = 2 10 6 9000 Small
PEDESTRIAN = 3 30 20 9000 Small
AUTOMOTIVE = 4 84 15 6000 Medium
SEA = 5 25 5 500 Medium
AIRBORNE_1G = 6 100 100 50000 Large
AIRBORNE_2G = 7 250 100 50000 Large
AIRBORNE_4G = 8 500 100 50000 Large
Note that it is obvious that the authentic DJI GPS uses a lot more filtering than Airborne1G. Observe the size of the pattern of the GPS trail in OpMap (diagnostics on, quad stationary on the ground) with the DJI and see a much smaller pattern than Ublox Airborne1G. The clone DJI that I have has a much larger pattern and does not exhibit the oscillation issue.
It is not the mag data. At least the issue does not go away if I use an I2C mag with a DJI GPS. I have two mounting positions (near and far) for GPS/mag. The Ublox/mag works well in either location. The DJI has the same oscillation in either location. Without some code changes there is no way to run the DJI mag with a Ublox GPS.
The issue exhibits as a slow (period of 4 seconds) oscillation in the east - west direction, when hovering in VelocityRoam flight mode. Oscillation is always east-west so it appears there is a separate issue hiding. Perhaps the longitudinal compression of GPS coordinates is not handled correctly somewhere, or the Ublox GPS does not compensate for it in it's velocity data. Oscillation is worse when the quad is pointing some directions than other directions. Another user with similar hardware has the same issue. I did not notice this issue when flying the same quad, but with GPSv9 using 15.09+.
The issue seems to be that the GPS data is effectively delayed, and we don't have a good way to handle that. I tried adjusting horizontal PID in VtolPathFollowSettings, and by the time I got the oscillation controlled, the quad was very unresponsive. Assuming that the filtering can be modeled as an integration, I am currently attempting to add some D term to the GPS data.
How do you tune an EKF to compensate for sensor lag? It seems that to handle it well, you must add code that handles the sensor delays. Tuning is ineffective.
I found that some modern quad EKFs attempt to handle various sensor delays:
http://ardupilot.org/dev/docs/extended-kalman-filter.html#ekf-pos-delay
An older article with useful ideas, of course there are many more:
http://www.dtic.mil/dtic/tr/fulltext/u2/a271334.pdf
Update 2017-01-24:
With authentic DJI/Naza GPS/mag (AuxOnly) I have done everything possible to make the east-west oscillation go away, but I have been unsuccessful. I recalibrated mag several times. I measured mag distortion with individual motors. I rewired the quad to make sure the mag fields were not an issue. I put the GPS/mag up higher on a pole. I tried adjusting "rotate virtual" to get Complementary / Attitude hovering without drift, then adjusting mag board rotation to get INS13 / Attitude hovering without drift. I did a code change to reduce GPS east velocity assuming longitudinal compression is not handled inside GPS.
If I replace the authentic DJI GPS/mag with a clone it works fine. If I replace the authentic DJI GPS/mag with an OP GPSv9 it works fine.
I seem to notice that even with other GPS brands, it rings a little more east-west than north-south?
It happens worst when the quad is pointing north. Oscillations increase when pointed north. Oscillations decrease in other directions.
older description follows
===================
User reports oscillation gets larger and larger and will eventually cause a crash. I also experienced that but found that reducing the VtolPF PIDs reduced and bounded the oscillations.
https://forum.librepilot.org/index.php?topic=2589.msg18484#msg18484
My initial guess is that this is from an unhandled east - west GPS coordinate/velocity compression that increases as you leave the equator. I see that we convert GPS LLA coordinates into ECEF, so it doesn't seem likely there, so my first tests will be an adhoc scaling of GPS NED velocity (E term) by latitude with the assumption that the reported velocity is too small and so it flies too fast and overshoots position. Maybe it is only the DJI GPS and happens because the reported velocity is not so compensated in the protocol, perhaps for historical reasons.
It could also be that there is something wrong in the ECEF conversions or that ECEF is incorrectly applied somewhere. It's also possible that it is related to the mag and EKF via east - west tilt being harder to see than north - south tilt.
There is a forum thread where other people have this with other GPS's (from before DJI code), but their oscillations are ~16x faster. Call it 4/second attitude oscillation but DJI is a 4 second period and an attitude/position oscillation. I think I noticed a quick attitude oscillation today as well. I don't recall if it was DJI authentic or DJI clone. The fast oscillation may only happen with tightly tuned PIDs.