Troubleshooting Kalman Filtering for INS/GPS Integration

[shtuceron]

Hello all.

I have some problem with realisation Kalman filtering in INS/GPS integration system.

I use equation from "Strapdown Inertial Navigaation" chapter 12. and trying programming Kalman filtering (discret), but have some trouble, estimate error have very small value on graphic, but error graphic have best result, why is it?

for example:

(if i zoom it is really good)Kalman filtering algorithm:

S = F*P*transpose(F) + G*Q*transpose(G);

K = S*transpose(H)*(H*S*transpose(H) + R)^-1;

Pout = (eye(15,15) - K*H)*S;

EXout = F*EX + K*(Z - H*F*EX);

 

[shtuceron]

It seems that I can find mistake, I use dynamic matrix from chapter 12, but on 404 page (chapter 13) wrote, that:"in Oder to allow a discrete Kalman filter to be constructed, it is necessary to express the system error equation (13.1) in discrete form. If ex represents the inertial navigation system error states at time tk and t(k+1) the error states at time t(k+1) we may write: dx =FF*dx + wk, where FF is the system transition matrix at time tk, which may be expressed in terms of the system matrix F as follows: FF = exp[F(t(k+1) - tk)]"
my question : how can I realize FF = exp[F(t(k+1) - tk)] in programming?

 

[shtuceron]

Thanks, i'am solve my problems.