2) So here the little arrows show the INS estimate of velocity and the position of the arrow is the current estimate of position. You can see there the arrows seem to run in the wrong direction and then jump back (when a GPS position update occurs) which is consistent with that yaw problem described previously. For longer runs in the same directions eventually the GPSVEL pulls the INS into the correct estimate.
3) And here to confirm that I plot the yaw estimate versus the raw magnetometer. Clearly they are barely related. So now the first priority is getting the INS tuned (and possibly modified) to make this more robust.
And here is one where it didn't:
What is striking in the one where it didn't is the INS velocity does _not_ point in the direction of the change in position. Basically it already thinks it's moving in the desired velocity so doesn't apply much attitude to fix it.
When I overlay the GPS velocity:
it appears the GPS velocity is fairly accurate so it needs to be trusted more in the EKF. Also I'd like to figure out why the INS ends up pointing the wrong way - whether the heading estimation is failing so acceleration is getting integrated in the wrong direction or if it is just noise.
It looks pretty crowded cause it stayed within 2 meters. I burned through two batteries with these settings without any problems which is nice. You can see the INS and GPSVEL arrows consistently point in the direction of the path.
There's also a new mode that lets the attitude come from the complimentary filter which is almost working which I think might make things even better for quads. I'll repeat tomorrow and get some video.
Notice when it swings really far to the north east that it seems like the EKF gets relatively disoriented for a bit. Because it already thinks it's mostly heading in the direction desired that means it won't try and tilt the quad that way.
In this one I think the EKF is actually doing quite a good job of staying ahead of the GPS so it's actually probably doing the right thing. Either way, you can see it's moving quite a few meters. The precession needs more looking into - it _could_ be because of heading error but I'm not entirely convinced. I'll try and add some code to see which way the quad seems to be tilting and figure out what is consistent. It might be that we want separate velocity PID settings for the component in the direction of the target and the normal.
Here I overlaid the "drive" arrows which is the direction it is tilted. As expected you can see those basically would go from the current to the desired velocity. It's definitely my impression that it isn't trying hard enough to suppress the momentum in the normal direction. However, within the space I've searched simply increasing the current PID settings doesn't seem to help much.
I'm thinking maybe more velocity Kd for damping? Second set of PIDs? Kenz ... suggestions?