Kalman Filter error-state model for psi-angle model.
I've been battling a problem for a while and have finally caved in and decided to look for help. The basic issue is the state system matrix for the psi-angle error model for a Kalman filter isn't coming together. I've developed several other filtering setups that have been based of modeling the system state as opposed to the error state, but have since switched for a variety of reasons.
The mathematics originally developed in [The Psi-Angle Error Equation in Strapdown Inertial
Navigation Systems, Benson 1975] makes sense although it does end up being a little convoluted. I've been trying to keep it being a linear relationship in order to preserve the ability to run it on a lower cost microcontroller (as opposed to being forced to calculate jacobians and the sort with an EKF). I've initially tried to reproduce the work in:
http://www.gmat.unsw.edu.au/snap/publications/liw&wang2012a.pdf
This however has proven difficult, their system matrix has no way of updating the Psi angles. If the components relating Psi(i) to Psi(i-1) are all zero in the system matrix their associated Kalman gains will also remain zero. The measurement model is easy and intuitive to follow and I have since rederived it for the purposes of my own bettering. The covariance terms for the system matrix can also be obtained through the Allan Variance Analysis. I've tried to overcome this by setting the diagonal terms relating Psi(i) to Psi(i-1) to one, but have had little success with utilizing this model. The one thing I did try however was looking at how Bias was modeled and it seemed to be that for the bias to be represented as a constant it should be integrated so I added a term that multiplied the the bias components of the State vector by the sample time as well so it became R*dt*G. This yielded:
With the following error state plots:
Sorry for not labeling them. Green is bias, red is psi, purple is the scale factor error (Initially kept low) and the Blue curve is the actual difference between the Gyro and Accelerometer. So this seems to make sense aside for the areas where psi is zero even though there is much apparent noise. The bias seems to follow the offsets of the blue curve fairly well.
I still feel very uncomfortable doing this as numerous papers have used the psi angle model without my modifications which I feel may just coincidentally be working. So I was hoping someone had some insight into the issue!
Replies to This Discussion
-
Permalink Reply by Bot Thoughts on -
I wished I did but am very interested in the discussion.
-
Permalink Reply by Leo on -
My initial thought is that I'm currently confusing the error model which relates the time derivative of Psi to to W(t) X(-Psi) - Bias to the System Matrix which should predict the next state of Psi as opposed to the rate of change of Psi.....working it out with my buddy as we speak. I think the real question becomes, given the matrix form of the psi-angle error model. What should the system matrix relating the previous to the current state of the model be?
As an aside I intend on releasing all of this code once it is finished as this has been so troublesome that I'd hate for anyone else to go through this hah.
-
Permalink Reply by Paul Phillips on
-
Hi There,
I have been reading the same paper from UNSW for the last few days and am slightly confused about a similar issue to the one you mentioned here. Is it not the predicted rate of change of Psi gives the predicted Psi at the next time step?
I was wandering if you had any luck since you made this post?
Regards,
Paul
-
Permalink Reply by Mohamed Zahana on -
Hey all,
I guess I am posting late here, but this is the very discussion that I need to get involved with. I just finished reading the paper, and would like to implement this algorithm in MATLAB, for example. Any body finished this problem and ready to help??
I will be really appreciating any help!
Thanks.
-
Permalink Reply by Leo on
-
Yup, all solved and works perfectly. Inconsistent notation and nonclear steps were the culprits. If I remember write, what looked like matrix operations were pointwise ones. That was the crux of the problem.
-
Permalink Reply by Mohamed Zahana on
-
That's so great!
Can you help me by providing the code, I'd like to try it.
Thnx
-
Permalink Reply by Leo on
-
Don't have it handy anymore, but I can dig through and see if I can find it.
-
Permalink Reply by Mohamed Zahana on
-
I will be so grateful, since I am in big need for it in order to finish a sub-task in my research so I can proceed towards more advanced control, You will be a life savior!!
Thnx again.
-
Permalink Reply by Mohamed Zahana on
-
Dear Leo,
I also had a question regarding your implementation. Didi you use any specific hardware, or just simulation?
-
Permalink Reply by Mohamed Zahana on
-
Dear Leo,
if you cannot find your implementation, can you help me to understand the paper in order to be able to implement it myself.
Thanks.
-
Permalink Reply by Mohamed Zahana on
-
Dear Leo,
Can I contact you be email to discuss some concepts in the adopted paper?!
Thanks.
Social Networking
Top Content
Contests
Season Two of the Trust Time Trial (T3) Contest has now begun. The fourth round is an accuracy round for multicopters, which requires contestants to fly a cube. The deadline is April 14th.A list of all T3 contests is here.
Groups
-
Moderators
24 members
-
ArduCopter User Group
1280 members
-
Andropilot Users Group
182 members
-
ArduCopter Italian Group
87 members
-
Team Prometheus
7 members
Advertisement
© 2013 Created by Chris Anderson.
Powered by
