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:
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!
I wished I did but am very interested in the discussion.
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.
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?
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!
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.
That's so great!
Can you help me by providing the code, I'd like to try it.
Don't have it handy anymore, but I can dig through and see if I can find it.
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!!
I also had a question regarding your implementation. Didi you use any specific hardware, or just simulation?
if you cannot find your implementation, can you help me to understand the paper in order to be able to implement it myself.
Can I contact you be email to discuss some concepts in the adopted paper?!