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.



Views: 4919


Replies to This Discussion

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.

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.

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.

Bests, Marco

hi Jani,

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.

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.


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.



Hi Marco,

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 :)



Ozan, this is really good work. The best way to integrate code into the official Ardu* codebase is to work within the dev team. Would you like to join? If so, please PM me your email address and I can add you to the dev list so you can get a sense of how it works.



Hi Chris,

i would love to join to dev team. I will send you my email address.



oh yay! more devs, we like more devs!



Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service