Hello, I am writing a Kalman Filter to estimate a vehicle position, and I am running into problems with the state covariance matrix and kalman gain. My state (simplified), is as follows:

The prediction phase is performed using hypothetical IMU input (at this point I am just using angular velocity). Graph 1 represents the expected pitch vs time, (Green), with the Kalman Output (Blue). Although the graph looks reasonable, there is a scope attached which shows the diagonal of the Covariance Matrix P corresponding to the pitch term.

So the Covariance Matrix is exploding and I don't know why. The covariance matrix changes in the Correction Phase, according to the Equations:

Code:

R = .005*eye(3); % only p,q,r are being corrected
i1=7; i2=9; %indices
xCorrect = x(i1:i2);
pCorrect = p(i1:i2,i1:i2);
H = eye(3); % p,q,r only
K = pCorrect*transpose(H)*pinv(H*pCorrect*transpose(H)+R)
xCorrect = xCorrect + K*(zTmp-H*xCorrect);
pCorrect = (1-K*H)*pCorrect;
x(i1:i2)=xCorrect;
p(i1:i2,i1:i2)=pCorrect;

And in the Prediction Phase according to a Very complicated Jacobian that I could not possibly write here. The only reason I think the problem lies in the Correction Phase is that, in the Prediction Phase, the values (of yaw at least) are predicted correctly according to the assumed system model... and the Jacobian derives directly from this model, with a call to the Matlab jacobian function.

The noise induced in the IMU data is zero mean Gaussian random noise with 0.05 Std.Dev. and my Initialization P value is 0.5*Identity.

Finally, My state is initialized correctly, and the Q value used for prediction is 0.05*Identity.

The only clue I am following is that the first output after initialization goes to zero along the diagonals, then explodes.

Thank you in advance for any help, I have lost some serious time trying to figure out this problem.

-Phil