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, 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.
i would love to join to dev team. I will send you my email address.
oh yay! more devs, we like more devs!
Welcome in dev group Ozan :)
Thank you Robero, soon i will offer my other suggestions for v2.6. Keep update.
my next suggestion for Arducopter code v2.6 is about compensation of unwanted magnetic effects such as hard-iron, soft-iron and scaling/calibration effects.
As we know that, since earth magnetic field is locally constant , the locus of all data points taken while a 3D magnetic sensor is rotated around every orientation, must be a sphere centered at origin. But in reality, it is generally a arbitrary rotated elipsoid centered at some offset from origin. Hard-iron effects, scaling errors and soft-iron effects are reasons for the offset, the elipsoidal shape and rotation of the elipsoid around some arbitrary axis, respectively.
Today, i rotated my arducopter with hand and meanwhile collected raw magnetic data from Diydrones magnetic sensor using logs of AP Mission Planner (after setting magnetic offsets to zero initially). What i found is my sphere was depressed on polar caps in the direction of z axis. I wrote a matlab function (see attachement) to find an elipsoid least square fit to the raw magnetic data. My results are below (See attachment for details).
Output parameters of matlab function (ellipsoid_fit2magnetic_data.m) are offsets and a transformation matrix. After translation of all data points using offsets to the origin and then applying transformation matrix (inverse W) , all magnetic raw data lying on the elipsoidal surface can be transformed to a sphere centered at origin eliminating hard-iron, soft-iron and scaling effects. Offsets i found are nearly similar to what AP mission plannner found. But for different calibration trials, contrary to my algorithm, live calib in AP mission planner gives slightly different results which i suppose was the result of using sphere least square fit (Tridge's algorithm) neglecting soft iron and scaling effects. As far as i know, Bill's latest automatic magnetic offset calculating algorithm in the arducopter v2.5.5 also neglects soft-iron and scaling effects. Someone may think that Diydrones magnetic sensor's built-in calibration deals with scaling error, however what i found says opposite.
My suggestion is to use elipsoidal fit algorithm in Mission Planner for the additional compensation of scale error and soft-iron effects and include transformation matrix into the code of arducopter v2.6, and then use Bill's algorithm. I suppose efficiency of algorithm will be superior.
Actually what i described here is not limited with 3D magnetic field sensors , it can be also applied to 3D accelerometers for calculating sensor offsets since gravitational field of earth is also constant locally. I also experienced scale errors for 3D accelerometers before. But data acquisition for compensation of accelerometer hard,soft and scale effects will be difficult unless eliminating inertial accelerations during the recording of accel data with different orientations.
Very nice job O.A.!
I hope all this integration in the future code, i can't wait to test it!
Hi Ozan: welcome to dev´s. I´ll give a try to the Quaternion-DCM. I guess it´s enough with substitute the two files in the AHRS lib.
Thanks for your ideas.
I´ve loaded your code in a APM1 and made some hand test with MP and works great. My impression is that reacts quicker and precisely than DCM code (It´s only an eye impression). I tryerd to torture it a lot and leave long time to see if it drifts and ...all OK. I´ll give a fly try this afternoon.
Yes it is enough, but you must manually built it. I will update arducopter git soon with final status.
I am happy to hear that finally someone wants to give it a try :)
I also observed norm of quaternions without any renormalization statement in the code, there was really super slow change in norm like 1 million part per second. Normalization with taylor expansion is more than enough. I use it now. I also compared two algorithms with moderate turn rates, there are even differences up to 1 degrees on the average which can be important for the stability of quad. With very slow rates there are indentical. I found that DCM algorithm underestimates angles especially with high angular velocities. There is also smaller numerical biases with DCM+quaternion hybrid algorithm since is intrinsically conserving magnitude of quaternions and is an exact solution, that's not linear approximation of kinematic equations.
Unfortunetly i have a electronic problem with my card APM1 (No receiver inputs). I ordered new one with atmega 2560 and waiting for it now.
Ok it seems a very good algorythm!
Just a question : there is a Nasa patent on it.
What about the use of this algorythm for us? Is it Ok?
Patent was taken in 2000. Probably there is time limit for obsolescence (10 year or so). And it is a mathematical algorithm which can be derived very easly. I don't understand why someone needs to take a patent on it. Also for non-profit or non-military usage, i dont think it will problem. you may not use it :)