Re: [Paparazzi-devel] Problems with ahrs Kalman

[Patrick McColl]

Attached is the source code of my ahrs, optimized eliminating all loops and 
calculating only the upper half of P and them filling up the rest by 
symmetry.

The original code (and probably easier to understand) is ahrs_new.c at

http://www.koders.com/c/fid22E0EE64D96CDDB738EBC21735FBFFC0749F5269.aspx

I have done a lot of tests and debugging, the filter works fine when static 
in any attitude but fails under motion after some minutes, even with pitch, 
roll and yaw under +-45 degrees.

I setup a microSD card to record debugging info 10 times per second.  I kept 
track of P but I don't have the knowledge to analyze it properly.

Something that I found not right is that P[0][0] starts at 1 (initial setup) 
and only decreases when in motion and tends to stabilize when stable.  After 
some time under motion it turns negative and decreases rapidly then 
instantly turns positive again. That is when the filter turns unstable.

I think the problem is with splitting the filter update into pitch, roll and 
yaw.

Is anybody currently using a similar Kalman filter that is working fine?

Thanks,

Patrick


--------------------------------------------------
From: "Felix Ruess" <address@hidden>
Sent: Friday, March 18, 2011 4:15 AM
To: <address@hidden>
Subject: Re: [Paparazzi-devel] Problems with ahrs Kalman

> Hi Patrick,
>
> yes the P matrix is symmetric, and you are right that this property
> should be use to optimize the code. This would also aid numerical
> stability.
> Which ahrs-kalman code are you referring to exactly?
>
> Cheers, Felix
>
> On Fri, Mar 18, 2011 at 12:45 AM, Todd Sandercock
> <address@hidden> wrote:
> > Koders is getting source from the old CVS right??? Try pulling the latest
> > source off of the GIT
> > Todd
> > ________________________________
> > From: Patrick McColl <address@hidden>
> > To: address@hidden
> > Sent: Fri, 18 March, 2011 9:46:37 AM
> > Subject: [Paparazzi-devel] Problems with ahrs Kalman
> >
> > Hi,
> >
> > I'm using the ahrs-Kalman code  (paparazzi3 version from koders..com) on 
> > a
> > board of my own design.  The board uses Rabbit BL4100 microprocessor 
> > board
> > and Analog Devices ADIS16364 gyro/accelerometer and Micromag3 magnetic
> > sensors.
> >
> > The code works fine with small movements but turns unstable after a 
> > minute
> > (or so) when I move the board simulating a normal flight.
> >
> > Has anybody experience the same problem?  Is there any way to fix it?
> >
> > While debugging I noticed that the P matrix is symmetric all the time. Is
> > this correct?
> > If so, the code can be optimized by calculating only half the matrix and
> > just filling the other half with the calculated values.
> >
> > Thanks,
> >
> > Patrick
> >
> >
> > _______________________________________________
> > Paparazzi-devel mailing list
> > address@hidden
> > http://lists.nongnu.org/mailman/listinfo/paparazzi-devel
>
> _______________________________________________
> Paparazzi-devel mailing list
> address@hidden
> http://lists.nongnu.org/mailman/listinfo/paparazzi-devel
>
>
> -----
> No virus found in this message.
> Checked by AVG - www.avg.com
> Version: 10.0.1204 / Virus Database: 1498/3513 - Release Date: 03/17/11
Patrick McColl Crozier
Gerente de Proyectos
Industrias Eléctricas RMS SA
Teléfono +56-32-2911446
Fax +56-32-2913301
web www.rms.co.cl

ahrs_utils.h
Description: Binary data

ahrs.C
Description: Binary data