Thread: Kalman Gain

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

Originally Posted by phil0stine

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

Why are you just updating the rate terms, you should be updating the entire state and covariance.

CB

Originally Posted by CaptainBlack

Why are you just updating the rate terms, you should be updating the entire state and covariance.

CB

I am glad that you mention that, I had been wrestling with this concept before deciding to just update part of the state and covariance.

My thinking was that :

1) I was (and still am) unsure as to how to declare my R matrix when using the entire state. Do I change the matrix based on what Correction data I have received? I thought the R matrix was fixed...

For example, if I had a GPS that sent x,y,z data with a certain covariance, I would not want to add those R-matrix terms when I am only correcting IMU data.


2) Since the only members of the state vector being Corrected were the rates, the result would be the same if I used only the diagonal 3x3 sub-matrix from the P and the sub-vector from x.

I did realize that this was only valid if the off-diagonal elements in P were zero. P must at least be (sym?) positive semi-definite, and I thought that the elements were uncorrelated...guess not, since my P matrix has off-diagonal elements everywhere.

So, I think I have just enough of an idea of the theory of whats going on to get myself in trouble. Thank you for your help, I will work some more on it today but I appreciate any theoretical/practical clarifications.

-Phil

Originally Posted by phil0stine

2) Since the only members of the state vector being Corrected were the rates, the result would be the same if I used only the diagonal 3x3 sub-matrix from the P and the sub-vector from x.

Because after propagating the state the off-diagonal terms of the covariance matrix for at least the attitude-rate if not the position-atitude-rate terms will become non-zero a correction even if you only have a measurement of the rates will change the other elements of the state as well as the rates.

CB

I have corrected the method and am now correcting the entire state, but I am having the same problems with the state covariance matrix.

To start from scratch, I am inputting IMU values with no randomness. Then I set the Q and R values to zero.

When I just predict using the Jacobian, the result is ideal as shown in the first figure. The small difference can be explained by the non-zero P value, but the P value remains constant throughout as expected.

The Kalman gain is on the order of , but I don't use it (I don't change the value of P at all during Correction Phase, and x is given by the new measurement)

When I incorporate the K value into x and P, the P value again explodes and the x-value output is shown in the second figure (which makes perfect sense since P is exploding). The K values are on the order of .

The new code is shown below

Code:

      K = pTmp*transpose(H)*pinv(H*pTmp*transpose(H)+R)
               xTmp = xTmp + K*(zTmp-H*xTmp);
               pTmp = (1-K*H)*pTmp;
               xOut = xTmp;
               pOut = pTmp;

Am I right to think that the problem does not lie in the Prediction Phase? The system has been so simplified at this point that I don't know how to make it moreso. Thanks in advance

Phil

Originally Posted by phil0stine

The new code is shown below

Code:

      K = pTmp*transpose(H)*pinv(H*pTmp*transpose(H)+R)
               xTmp = xTmp + K*(zTmp-H*xTmp);
               pTmp = (1-K*H)*pTmp;
               xOut = xTmp;
               pOut = pTmp;

1-K*H is not the same as I-K*H so try replacing it with eye(size(pTmp)(1)-K*H (or whatever works)

CB

Perfect, here is the result. The scope on P and the graph of x are attached.

Thank you very much for taking the time to give it a look. It was a silly error, but I still appreciate it.

-Phil