hi Arducopter fans there,
i will try to give some suggestions regarding arducopter v2.6 before its release. Since there has been some time without any improvement about AHRs algorithms over recent arducopter versions. I think, the best place to begin is DCM algoritm itself.
My suggestion is to combine DCM and quaternion methods with their most superior parts. I know there has been a marg quaternion algorithm, but it is unnecessarily complicated. We know that DCM is easy to understand and has a better implemention for drift correcting algorithms and magnetic offset calculations. But DCM algorithm needs to orthogonalize and normalize rows or colums of the direction cosine matrix, also it is slightly inefficient due to 9 elements of the matrix is to be updated. As for quaternion method, only one vector of element size 4 needs to be updated and normalized. Actually, it is possible to integrate quaternions while preserving their unity. There is an algorithm for it. A DCM matrix, which can be calculated from updated quaternion will have aldready orthonomalized rows and columns. So i think it will be much more efficient and logical to do attitude update using quaternions with unity preserving integration and then calculate DCM representation from the updated quaternions and use it when necessary .
I have already implemented my DCM-quaternion hybrid suggestion. It can be easly integrated to the arducopter v2.5.5 code just using new versions of AP_AHRS_DCM library which i put in the attachments. I hope we will have more stable loiter and waypoint navigation in the future.
Ozan, we already tried quaternion several months ago and results were not as good as we expected. If you look our source code, you can still see custom quarternion compile time flags there and if you wan you can test them by your self too. But you need to compile codes manually.
After 1.5 months of endles work devs stopped to work with code at least for now. But if you have some more suggestions. Post them here and we can let our devs to check them out.
i am well aware of quaternion code you tried (AP_AHRS_Quaternion.cpp). But it depends on Jeb Madgwick's algorithm, does it? Actually i have not tried it yet and don't think to try it, because algorithm itself depends on many assumptions or not easy to implement and tune. Also i dont smell any success.
What i suggest here here is slightly different. It is a explicit complementary filter like DCM but just based on quaternions. I tried it before adding unity preserving integration with fixed point calculations in Bill's red uav devboard and it was really superior. Even in fixed point case, norm of quaternions stays unity in a long term, but because of numerical resolution (fixed point) ,it is supposed to renorm quaternions anyway but error in norm was still much more less. In floating case, it will be much efficient in conserving unity.
I have been trying in my arducopter now and flying with it, it reduced the code size 416 byte already. I have not seen any eye caching difference so far (Probably due to floating point), but we know than DCM is only true for small angular rates. But what i offer is exact solution due to exact true integration in quaternion space. I suppose it will show difference when it is exposed to high turn rates. It will be also good idea to test it SIM implementation using the code i attached before.
Yes, recent updates are about drift calculations parts, but state update and orthonormalization are still old the way around (DCM).
In DCM and quaternion hybrid code, update and orthonormalization are handled at the same time with out any assumptions. I have been using it, and no problem so far. In DCM only code, update with unity integration can also be done using rodrigez's formula, however much hard to implement in this case in comparison to quaternions.
Wow, nice shot! Are you from NASA?
Did you already try this on your copter or is it just a SIM implementation?
@Jani I think this is slightly different from what tried till now.
This is a Hybrid implementation using Quaternions and DCM.
Will have a look later and maybe try it on the SIM.
emile, yeah looks like. Already informed our dev teams to check. OA will be contacted soon about this. So let's keep working.
I wish i was from NASA. :)
Yes i tried it today. First just with my hands shaking and checking using AP mission planner. Then i flied with it. No problem so far.
Thanks Ozan for your suggestion, I pointed out this discussion on dev list.
Can you integrate your update in the latest GIT code? Have you tried this mod in flight?
I mean if there's substantial difference in the DCM library, i really want to try it.
Of course, i can integrate my update in the latest GIT code. But i am unfamiliar GIT code development, it may take some time.
There is 410 byte reduction of code, integration is very easy. Almost most of the code is intact. You try it and see for your self :)
In order to investigate performance of DCM+Quaternion hybrid, i implement it with conventional DCM based imu algorithm. I run both algorithm in arducopter v2.5.5 code at the same time with same gyro and accel vector but independently. Her are the results (Compass disabled)
As i suspected, real difference can be seen when shaking with high angular rates. DCM+quaternion with unity preserving integration gives better performance. I will update my branch in arducoter GIT code asap.
Hi Ozan have you a link or a reference of that on the Nasa site?
Here it is, i have already implemented, you can also refer to library in my old attachment.