Results 1 to 7 of 7

Math Help - Kalman Gain

  1. #1
    Newbie
    Joined
    Mar 2009
    Posts
    8

    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:

    X=[x,y,z,roll,pitch,yaw,p,q,r]'

    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


    Attached Thumbnails Attached Thumbnails Kalman Gain-pitchoutput.png   Kalman Gain-pgraph.png   Kalman Gain-pgraphzoom.png  
    Follow Math Help Forum on Facebook and Google+

  2. #2
    Grand Panjandrum
    Joined
    Nov 2005
    From
    someplace
    Posts
    14,972
    Thanks
    4
    Quote Originally Posted by phil0stine View Post
    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:

    X=[x,y,z,roll,pitch,yaw,p,q,r]'

    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
    Follow Math Help Forum on Facebook and Google+

  3. #3
    Newbie
    Joined
    Mar 2009
    Posts
    8
    Quote Originally Posted by CaptainBlack View Post
    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
    Follow Math Help Forum on Facebook and Google+

  4. #4
    Grand Panjandrum
    Joined
    Nov 2005
    From
    someplace
    Posts
    14,972
    Thanks
    4
    Quote Originally Posted by phil0stine View Post
    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
    Follow Math Help Forum on Facebook and Google+

  5. #5
    Newbie
    Joined
    Mar 2009
    Posts
    8
    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 10^3, 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 10^-1.

    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
    Attached Thumbnails Attached Thumbnails Kalman Gain-idealnogain.png   Kalman Gain-idealwithgain.png  
    Follow Math Help Forum on Facebook and Google+

  6. #6
    Grand Panjandrum
    Joined
    Nov 2005
    From
    someplace
    Posts
    14,972
    Thanks
    4
    Quote Originally Posted by phil0stine View Post
    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
    Follow Math Help Forum on Facebook and Google+

  7. #7
    Newbie
    Joined
    Mar 2009
    Posts
    8
    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
    Attached Thumbnails Attached Thumbnails Kalman Gain-pscope.png   Kalman Gain-output.png  
    Follow Math Help Forum on Facebook and Google+

Similar Math Help Forum Discussions

  1. aerial state model kalman
    Posted in the Advanced Statistics Forum
    Replies: 0
    Last Post: June 9th 2009, 08:45 AM
  2. critical gain
    Posted in the Calculus Forum
    Replies: 1
    Last Post: June 8th 2009, 01:29 PM
  3. Kalman filtering application
    Posted in the Advanced Applied Math Forum
    Replies: 54
    Last Post: April 7th 2009, 09:08 PM
  4. Account for latency in Kalman Filter
    Posted in the Advanced Statistics Forum
    Replies: 2
    Last Post: March 16th 2009, 03:56 AM
  5. System identification using Extended Kalman Filter
    Posted in the Advanced Applied Math Forum
    Replies: 2
    Last Post: December 26th 2007, 07:20 PM

Search Tags


/mathhelpforum @mathhelpforum