Kalman Filtering: Gain Determination help

I'm relatively new to using Kalman filters. To get my feet wet, I'm working on a program simulating a sounding rocket with a barometric
pressure sensor to determine apogee. I've taken "ideal" altitude data and overlaid white noise to approximate a noisy pressure sensor. I've been able to "guess" (hardcode) gain values that allow the filter to track the data relatively closely, however I'm having trouble determining the correct gains (to minimize the error covariance)..
Based on the white noise I've used, the barometric pressure sensor noise has a stdev#.65 meters. I'm ballparking that the process noise only occurs in acceleration (wind, buffeting, etc), and has a stdev of 10 m/s^2. Therefore, the Q and R matricies that I'm using are:
Q=[0 0 0 0 0 0 0 0 100]
R=[559.5 0 0 0 0 0 0 0 0]
However, when the program encounters the code to calculate the new gains K, I get the following: "Warning: Matrix is singular to working precision".
Can anyone spot a mistake I'm making in determining Q or R? Below is the "meat" of the filter I'm using, in case that is the source of the errors. (note: I'm defining P as a 3 X 3 X "time interval" matrix)
Thanks in advance for any help or advice! Dave
--------------------------------------------------- while i<=off x1=A*X; X=x1+K*(y_meas(i)-x1(1));
P1=(A*P(:,:,i))*A'+Q; K=P1*H'*inv(H*P1*H'+R) P(:,:,i+1)=(I-K1*H)*P1;
i=i+1; XK(i,1)=X(1); XK(i,2)=X(2); XK(i,3)=X(3); end
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload
[Snip]

Hi Dave,
Have you initialized P properly? You must give it some initial estimate of the variance of the process. The determination of R and Q is not so important. The Kalman filter is very robust and you use R and Q for tuning, so even when R and Q are wrong it should work.
HTH
Robert
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload

Er, they're singular. :-)
Try:
eps = 0.01;
Q = [eps 0 0 0 eps 0 0 0 100];
R = [559.5 0 0 0 eps 0 0 0 eps];
and see what you get.
Ciao,
Peter K.
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload
Peter K. wrote:

Is singularity of Q and R really a problem? Since they describe the covariance of the state and the measurement it seems perfectly valid to have zero main diagonals.
According to the theory, Q and R have only to be positive semidefinite, which is the case here.
Maybe the system Dave is trying to estimate is not observable?
Regards
Robert
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload
Peter K. wrote:

That was exactly the problem. There were some other modifications (i.e. corrections) I made to the code, but that was reason it was giving me that error initially. The results I get are excellent. Thanks to all who provided insight.
On a side note, I'm wondering if I can compensate for non-vertical flights (say it launches at a 25deg angle)...? For non-vertical flights, the accelerometer measurement will be higher than the actual y-axis acceleration. The exact equation is A_vertical = A_measured * cos(angle), although I'm treating cos(angle) as a changing scalar. I'm toying with the idea of trying to estimate this scaler.
I was hoping to use the ratio of the change in velocity (i.e. the change in velocity between time step k-1 to k) with the direct accelerometer measurement. The pressure sensor input will keep the estimated velocity lower than the integrated acceleration. So, could this ratio be used as a 'virtual' sensor? I believe my new 'A' matrix becomes:
A=[1 dt X(4)*.5*dt^2 0 0 1 X(4)*dt 0 0 0 X(4)*1 0 0 0 0 1];
where:
X=[position velocity acceleration vertical_scalar];
And my scaler measurement (i.e. virtual sensor) is
s_meas(i)=(X(i,2)-X(i-1,2))/(a_meas(i-1)*dt);
I guess this is getting into Extended Kalman Filters? Anyways, is it possible for this method of estimating the non-vertical scalar work?
Thanks in advance for any insight! Dave
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload
I am doing some work using Kalman filter. The result is not right. There is some steady-state error. Is this because of the accuracy of the mathematical model? Thanks. Have a good day.
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload

Dear Tang
What is the system and what is the model?
Kieran

Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload

Polytechforum.com is a website by engineers for engineers. It is not affiliated with any of manufacturers or vendors discussed here. All logos and trade names are the property of their respective owners.