Kalman filter with adaptive measurement variance estimator

• US 20030115232A1
• Filed: 12/17/2001
• Published: 06/19/2003
• Est. Priority Date: 12/17/2001
• Status: Active Grant

• Current Assignee: Lockheed Martin Corporation (Martin Marietta Corporation)
• Original Assignee: Lockheed Martin Corporation (Martin Marietta Corporation)
• Inventors: Lipp, John
• Granted Patent: US 7,209,938 B2
• Time in Patent Office: Days
• US Class Current: 708/300
• CPC Class Codes: G06F 17/18 for evaluating statistical data , e.g. average values, frequency distributions, probability functions, regression analysis forecasting specially adapted for a specific administrative, business or logistic context G06Q10/04
• CPC Class Codes: H03H 21/003 KALMAN filters

Abstract

The invention is a Kalman filtering technique employing an adaptive measurement variance estimator. The invention includes a signal filtering mechanism, comprising a Kalman filter and a variance estimator. The variance estimation used in the filtering includes estimating the variance of the measured quantity signal and generating the variance estimate signal for use in filtering the input signal and the measured quantity signal, wherein estimating the variance of the measured quantity signal includes determining a smoothed estimate of the measured quantity'"'"'s variance from the measured quantity signal. The invention also manifests itself as a method for filtering and estimating, a program storage medium encoded with instructions that, when executed by a computer, performs such a method, an electronic computing device programmed to perform such a method, and a transmission medium over which the method is performed.

Claims

1. A signal filtering mechanism, comprising:

a Kalman filter capable of receiving an input signal, a measured quantity signal, and a variance estimate signal for the measured quantity signal, and outputting a state estimate signal; and

a variance estimator capable of estimating the variance of the measured quantity signal and generating the variance estimate signal for use in filtering the input signal and the measured quantity signal, wherein estimating the variance of the measured quantity signal includes determining a smoothed estimate of the measured quantity'"'"'s variance from the measured quantity signal.

2. The signal filtering mechanism of claim 1, wherein determining the smoothed estimate comprises:

determining the squared instantaneous prediction error of the measured quantity signal;

smoothing the determined, squared instantaneous prediction error; and

estimating the measured quantity'"'"'s variance from the smoothed squared instantaneous prediction error.

3. The signal filtering mechanism of claim 1, wherein determining the smoothed estimate comprises:

determining the absolute instantaneous prediction error in the measured quantity signal;

smoothing the determined, absolute instantaneous prediction error; and

estimating the measured quantity'"'"'s variance from the smoothed absolute instantaneous prediction error.

4. A method for estimating the variance of a measured quantity used to predict the current state of a discrete, vector-state, scalar-measurement system, the method comprising:

estimating the variance of a measured quantity for use in filtering an input quantity and the measured quantity;

determining a smoothed estimate of an instantaneous prediction error'"'"'s variance; and

filtering the input quantity and the measured quantity through a Kalman filter using the estimated input variance of the measured quantity signal.

5. The method of claim 4, wherein determining the smoothed estimate of the variance of the instantaneous prediction error of the measured quantity signal comprises:

determining the squared instantaneous prediction error in the measured quantity signal;

smoothing the determined, squared instantaneous prediction error; and

estimating the variance from the smoothed squared instantaneous prediction error.

6. The method of claim 5, further comprising initializing a plurality of quantities used in estimating the current state of the discrete, vector-state, scalar-measurement system.

7. The method of claim 6, wherein initializing the plurality of quantities includes:

setting the prediction of the initial state of the system to a first predetermined value; and

setting the initial prediction covariance matrix associated with the predicted initial state of the system to a second predetermined value; and

one of;

setting the error filter gain to a third predetermined value;

or

setting the smoothed squared instantaneous prediction error to 0.

8. The method of claim 7, wherein the third predetermined value is a vector of ones.

9. The method of claim 7, wherein the error filter gain is set to a third predetermined value and filtering the input quantity and the measured quantity includes determining a Kalman filter gain vector, a current state vector estimate, and a state vector estimate covariance matrix after estimating the variance of the measured quantity.

10. The method of claim 7, wherein the smoothed squared instantaneous prediction error is set to 0 and filtering the input quantity and the measured quantity includes determining a Kalman filter gain vector, a current state vector estimate, and a state vector estimate covariance matrix before estimating the variance.

11. The method of claim 4, wherein determining the smoothed estimate of the variance of the instantaneous prediction error of the measured quantity signal comprises:

determining the absolute instantaneous prediction error in the measured quantity signal;

smoothing the determined, absolute instantaneous prediction error; and

estimating the variance from the smoothed absolute instantaneous prediction error.

12. The method of claim 11, further comprising initializing a plurality of quantities used in estimating the current state of the discrete, vector-state, scalar-measurement system.

13. The method of claim 12, wherein initializing the plurality of quantities includes:

setting the prediction of the initial state of the system to a first predetermined value; and

setting the initial prediction covariance matrix associated with the predicted initial state of the system to a second predetermined value; and

one of;

setting the error filter gain to a third predetermined value;

or

setting the smoothed absolute instantaneous prediction error to 0.

14. The method of claim 13, wherein the third predetermined value is a vector of ones.

15. The method of claim 13, wherein the error filter gain is set to a third predetermined value and filtering the input quantity and the measured quantity includes determining a Kalman filter gain vector, a current state vector estimate, and a state vector estimate covariance matrix after estimating the variance of the measured quantity.

16. The method of claim 13, wherein the smoothed absolute instantaneous prediction error is set to 0 and filtering the input quantity and the measured quantity includes determining a Kalman filter gain vector, a current state vector estimate, and a state vector estimate covariance matrix before estimating the variance.

17. A method for estimating the current state of a discrete, vector-state, scalar-measurement system, the method comprising:

determining a current state vector prediction from a previous state vector estimate and an input vector;

determining a current state vector prediction covariance matrix associated with the current state vector prediction from a previous state vector covariance matrix associated with the previous state vector estimate;

estimating the variance of a measured quantity, wherein estimating the variance includes;

determining a squared instantaneous prediction error of the measured quantity from the measured quantity and one of the current state vector estimate and the previous state vector estimate;

smoothing the squared instantaneous prediction error; and

estimating the variance of the measured quantity from the smoothed squared instantaneous prediction error;

determining a current Kalman filter gain vector from the current state vector prediction covariance matrix and the measured quantity variance estimate;

determining a current state vector estimate from the Kalman filter gain, the current state vector prediction, and the measured quantity;

determining the current state vector covariance matrix associated with the current state vector estimate from the Kalman filter gain and the current state vector prediction covariance matrix; and

iterating the above.

18. The method of claim 17, further comprising initializing a plurality of quantities used in estimating the current state of the discrete, vector-state, scalar-measurement system.

19. The method of claim 18, wherein initializing the plurality of quantities includes:

setting the value of the current state vector prediction to a first predetermined value;

setting the current state vector prediction covariance matrix to a second predetermined value; and

performing one of;

setting an error filter gain to a third predetermined value;

or

setting the squared instantaneous prediction error to 0.

20. The method of claim 17, wherein the current state vector estimate, the current state vector prediction covariance matrix, and the current Kalman filter gain are updated after the variance of the measured quantity is estimated.

21. The method of claim 20, wherein, in estimating the variance of the measured quantity includes:

determining the squared instantaneous prediction error in the measured quantity includes applying the following analysis;

e²[n]=(z[n]−

H[n]{circumflex over (x)}[n|n−

1])²;

smoothing the determined, squared instantaneous prediction error includes applying the following analysis;

{circumflex over (σ

)}_e²[n]={circumflex over (σ

)}_e²[−

1]+H[n]G[n](e²[n]−

{circumflex over (σ

)}_e²[n−

1]); and

estimating the variance of the measured quantity from the smoothed squared instantaneous prediction error comprises;

setting the estimated variance to a fourth predetermined value if the Kalman filter is not stable;

or

applying the following analysis if the Kalman filter is stable;

determining the value of a[n] from;

$a<br><br>\left[n\right]=\frac{H<br><br>\left[n\right]<br><br>A<br><br>\left[n\right]<br><br>{H<br><br>\left[n\right]}^T}{H<br><br>\left[n\right]<br><br>{H<br><br>\left[n\right]}^T};<br><br>$

determining the value of q[n] from;

q[n]=(H[n]B[n])Q[n](H[n]B[n])^T; and

solving for {circumflex over (R)}[n] from

{circumflex over (R)}[n]²(2a²[n]−

1)+{circumflex over (R)}[n][{circumflex over (σ

)}_e²[n](1−

3a²[n])−

2q[n]]+{circumflex over (σ

)}_e²[n](q[n]+{circumflex over (σ

)}_e²[n]a²[n])=0.

22. The method of claim 17, wherein the current state vector estimate, the current state vector prediction covariance matrix, and the current Kalman filter gain are updated before the variance of the measured quantity is estimated.

23. The method of claim 22, wherein, in estimating the variance of the measured quantity includes:

determining the squared instantaneous prediction error of the measured quantity includes applying the following analysis;

e²[n]=(z[n]−

H[n]{circumflex over (x)}[n])²;

smoothing the determined, squared instantaneous prediction error includes applying the following analysis;

{circumflex over (σ

)}_e²[n]={circumflex over (σ

)}_e²[n−

1]+H[n](e²[n]−

{circumflex over (σ

)}_e²[n−

1]); and

estimating the variance from the smoothed squared instantaneous prediction error comprises;

setting the estimated variance to a fourth predetermined value if the Kalman filter is not stable;

or

applying the following analysis if the Kalman filter is stable;

determining the value of a[n] from;

$a<br><br>\left[n\right]=\frac{H<br><br>\left[n\right]<br><br>A<br><br>\left[n\right]<br><br>{H<br><br>\left[n\right]}^T}{H<br><br>\left[n\right]<br><br>{H<br><br>\left[n\right]}^T};<br><br>$

determining the value of q[n] from;

q[n]=(H[n]B[n])Q[n](H[n]B[n])^T; and

solving for {circumflex over (R)}[n] from

${\hat{R}<br><br>\left[n\right]}^4<br><br>\text{\hspace{1em}<br><br>}<br><br>\frac{a^2<br><br>\left[n\right]}{{\left({\widehat{\mathrm{\sigma <br><br>}}}_e^2<br><br>\left[n\right]\right)}^2}+{\hat{R}<br><br>\left[n\right]}^3<br><br>\text{\hspace{1em}<br><br>}<br><br>\frac{1-3<br><br>a^2<br><br>\left[n\right]}{{\widehat{\mathrm{\sigma <br><br>}}}_e^2<br><br>\left[n\right]}+{\hat{R}<br><br>\left[n\right]}^2<br><br>\left(2<br><br>a^2<br><br>\left[n\right]-1\right)<br><br>\left(1-\frac{q<br><br>\left[n\right]}{{\widehat{\mathrm{\sigma <br><br>}}}_e^2<br><br>\left[n\right]}\right)+\left(\hat{R}<br><br>\left[n\right]<br><br>3<br><br>q<br><br>\left[n\right]+q^2<br><br>\left[n\right]\right)<br><br>\left(a^2<br><br>\left[n\right]-1\right)=0$

24. A signal filtering mechanism, comprising:

means for receiving an input signal, a measured quantity signal, and a variance estimate signal for the measured quantity signal, and outputting a state estimate signal; and

means for estimating the variance of the measured quantity signal and generating the variance estimate signal for use in filtering the input signal and the measured quantity signal, wherein estimating the variance of the measured quantity signal includes determining a smoothed estimate of the measured quantity'"'"'s variance from the measured quantity signal.

25. The signal filtering mechanism of claim 24, wherein determining the smoothed estimate comprises:

determining the squared instantaneous prediction error of the measured quantity signal;

smoothing the determined, squared instantaneous prediction error; and

estimating the measured quantity'"'"'s variance from the smoothed squared instantaneous prediction error.

26. The signal filtering mechanism of claim 24, wherein determining the smoothed estimate comprises:

determining the absolute instantaneous prediction error in the measured quantity signal;

smoothing the determined, absolute instantaneous prediction error; and

estimating the measured quantity'"'"'s variance from the smoothed absolute instantaneous prediction error.

27. A apparatus for estimating the variance of a measured quantity used to predict the current state of a discrete, vector-state, scalar-measurement system, the method comprising:

means for estimating the variance of a measured quantity for use in filtering an input quantity and the measured quantity;

means for determining a smoothed estimate of an instantaneous prediction error'"'"'s variance; and

means for filtering the input quantity and the measured quantity through a Kalman filter using the estimated input variance of the measured quantity signal.

28. The apparatus of claim 27, wherein the means for determining the smoothed estimate of the variance of the instantaneous prediction error of the measured quantity signal comprises:

means for determining the squared instantaneous prediction error in the measured quantity signal;

means for smoothing the determined, squared instantaneous prediction error; and

means for estimating the variance from the smoothed squared instantaneous prediction error.

29. The apparatus of claim 27, wherein the means for determining the smoothed estimate of the variance of the instantaneous prediction error of the measured quantity signal comprises:

means for determining the absolute instantaneous prediction error in the measured quantity signal;

means for smoothing the determined, absolute instantaneous prediction error; and

means for estimating the variance from the smoothed absolute instantaneous prediction error.

30. A program storage medium encoded with instructions that, when executed by a computing apparatus, perform a method for estimating the variance of a measured quantity used to predict the current state of a discrete, vector-state, scalar-measurement system, the method comprising:

estimating the variance of a measured quantity for use in filtering an input quantity and the measured quantity;

determining a smoothed estimate of an instantaneous prediction error'"'"'s variance; and

filtering the input quantity and the measured quantity through a Kalman filter using the estimated input variance of the measured quantity signal.

31. The program storage medium of claim 30, wherein determining the smoothed estimate of the variance of the instantaneous prediction error of the measured quantity signal in the encoded method comprises:

determining the squared instantaneous prediction error in the measured quantity signal;

smoothing the determined, squared instantaneous prediction error; and

estimating the variance from the smoothed squared instantaneous prediction error.

32. The program storage medium of claim 30, wherein determining the smoothed estimate of the variance of the instantaneous prediction error of the measured quantity signal in the encoded method comprises:

determining the absolute instantaneous prediction error in the measured quantity signal;

smoothing the determined, absolute instantaneous prediction error; and

estimating the variance from the smoothed absolute instantaneous prediction error.

33. A computing apparatus programmed to perform a method for estimating the variance of a measured quantity used to predict the current state of a discrete, vector-state, scalar-measurement system, the method comprising:

estimating the variance of a measured quantity for use in filtering an input quantity and the measured quantity;

determining a smoothed estimate of an instantaneous prediction error'"'"'s variance; and

filtering the input quantity and the measured quantity through a Kalman filter using the estimated input variance of the measured quantity signal.

34. The computing apparatus of claim 33, wherein determining the smoothed estimate of the variance of the instantaneous prediction error of the measured quantity signal in the programmed method comprises:

determining the squared instantaneous prediction error in the measured quantity signal;

smoothing the determined, squared instantaneous prediction error; and

estimating the variance from the smoothed squared instantaneous prediction error.

35. The computing apparatus of claim 33, wherein determining the smoothed estimate of the variance of the instantaneous prediction error of the measured quantity signal in the programmed method comprises:

determining the absolute instantaneous prediction error in the measured quantity signal;

smoothing the determined, absolute instantaneous prediction error; and

estimating the variance from the smoothed absolute instantaneous prediction error.