High accuracy inertial sensors from inexpensive components

• US 20030187623A1
• Filed: 03/06/2003
• Published: 10/02/2003
• Est. Priority Date: 03/06/2002
• Status: Active Grant

• Current Assignee: California Institute of Technology
• Original Assignee: California Institute of Technology
• Inventors: Ploen, Scott R., Bayard, David S.
• Granted Patent: US 6,882,964 B2
• Time in Patent Office: Days
• US Class Current: 702/190
• CPC Class Codes: G01C 21/183 Compensation of inertial measurements, e.g. for temperature effects

Abstract

An analytical apparatus for optimally combining measurements from N individual rate sensing devices or gyros into a single rate estimate significantly improves performance over that of any individual component device. Kalman filtering is used to combine rate sensed devices optimally in the sense of minimizing the variance of the rate error. The Riccati differential equation (RDE) associated with combining a collection of rate sensed devices is completely and exactly solved to derive to the matrix RDE. This analytic solution serves as the key for understanding all of the theoretical properties of the optimal filter, and provides a complete characterization of the final virtual rate sensed performance. In addition, the analytic RDE solution allows many practical problems to be solved that have proved essential for developing successful filter implementations. A discrete-time minimum variance filter implementation combines sensor measurements optimally.

Claims

1. A method of processing signals from multiple rate sensing devices each providing a rate measurement to derive a higher accuracy output signal representing a best estimate of a virtual sensed rate, the method comprising:

measuring the rate measurements from multiple rate sensing devices;

combining the rate measurements from the multiple rate sensing devices in a recursive processing filter, wherein the weights and smoothing properties of the recursive processing filter are determined to advantage using statistical correlations and noise characteristics of the multiple rate sensing devices.

2. The method of claim 1 wherein combining the rate measurements from the multiple rate sensing devices is performed by a continuous-time steady state Kalman filter to generate a minimum variance rate estimate.

3. The method of claim 1 wherein combining the rate measurements of the multiple rate sensing devices comprises:

digitizing the rate measurements of the multiple rate sensing devices; and

digitally combining the rate measurements of the multiple rate sensing devices using a digital steady state Kalman filter to generate a minimum variance rate estimate.

4. The method of claim 3 wherein digitally combining the rates of the multiple rates sensing devices using a digital steady state Kalman filter comprises:

determining N, wherein N is a number of rate sensing devices being measured;

determining Q_b and R, wherein Q_b and R are N×

N covariance matrices whose elements correspond to the correlation factors between the N multiple rate sensing devices;

choosing a random walk intensity q_ω

for a true rate, and forming

$Q=\left[\begin{array}{cc}{Q}_b& 0\\ 0& q_{\mathrm{\omega <br><br>}}\end{array}\right],$

 

wherein Q is an (N+1)×

(N+1) covariance matrix;

generating L=Q^1/2H^TR<sup>−

1</sup>HQ^1/2, wherein L is an (N+1)×

(N+1) matrix, wherein H represents an N×

(N+1) matrix in a measurement equation y=Hx+v=b+1ω

+v;

wherein ω

represents a true rate along a single axis;

wherein b represents a vector of rate biases;

wherein x is a state vector x=[b^T,ω

)]^T;

wherein v is a vector of angle random walk (ARW) noises that have been stacked into the vector form v=[n₁₁, n₁₂, . . . , n_1N]^T; and

wherein y is a vector representing the measured signals from N gyros;

performing an eigenvalue decomposition of L ordered such that its zero eigenvalue is last wherein L=ADA^T, and wherein D is a diagonal matrix and A is an orthogonal matrix;

extracting a N×

N submatrix {overscore (D)} from the matrix D according to

$D=\left[\begin{array}{cc}\stackrel{\_}{D}& 0\\ 0& 0\end{array}\right];<br><br>$

extracting a (N×

1)×

N submatrix {overscore (A)} from the matrix A according to A=[{overscore (A)}v];

generating a steady state Kalman gain as

$\stackrel{\_}{K}=S<br><br>\left[\begin{array}{ccc}{\stackrel{\_}{D}}^{-1/2}& \left(I-{\mathrm{<br><br>}}^{-{\stackrel{\_}{D}}^{\frac{1}{2}}<br><br>T}\right)& 0\\ \text{\hspace{1em}<br><br>}& 0& 0\end{array}\right]<br><br>S^T<br><br>H^T<br><br>R^{-1}=Q^{1/2}<br><br>\stackrel{\_}{A}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{\_}{D}}^{-1/2}<br><br>\left(I-{\mathrm{<br><br>}}^{-{\stackrel{\_}{D}}^{1/2}<br><br>T}\right)<br><br>{\stackrel{\_}{A}}^T<br><br>Q^{1/2}<br><br>H^T<br><br>R^{-1}$

 

wherein S=Q^1/2A and wherein T is a sampling period; and

,

processing the measurements from the multiple rate sensing devices according to the recursive processing filter, the recursive processing filter having an output and modeled as,

$\widehat{\mathrm{\xi <br><br>}}<br><br>\left(\left(k+1\right)<br><br>T\right)=\left[\begin{array}{cc}{\mathrm{<br><br>}}^{-{\stackrel{\_}{D}}^{\frac{1}{2}}<br><br>T}& 0\\ 0& 1\end{array}\right]<br><br>\widehat{\mathrm{\xi <br><br>}}<br><br>\left(k<br><br>\text{\hspace{1em}<br><br>}<br><br>T\right)+S^{-1}<br><br>\stackrel{\_}{K}<br><br>\text{\hspace{1em}<br><br>}<br><br>y<br><br>\left(k<br><br>\text{\hspace{1em}<br><br>}<br><br>T\right)$$\widehat{\mathrm{\omega <br><br>}}<br><br>\left(k<br><br>\text{\hspace{1em}<br><br>}<br><br>T\right)=e_{N+1}^T<br><br>S<br><br>\text{\hspace{1em}<br><br>}<br><br>\widehat{\mathrm{\xi <br><br>}}<br><br>\left(k<br><br>\text{\hspace{1em}<br><br>}<br><br>T\right)$

 

wherein x represents a vector x=[b^T,ω

]^T;

ξ

(kT)=S<sup>−

1</sup>x(kT) is a filter state at the k^th sampling point;

$e_{N+1}^T=\left[0,\mathrm{\dots <br><br>}<br><br>\text{\hspace{1em}<br><br>},0,1\right]$

is a selection vector; and

{circumflex over (ω

)}(kT) is the output of the recursive filter which represents the best estimate of the virtual sensed rate for the k^th sampling point.

5. The method of claim 4 wherein S⁻

1{overscore (K)} is a matrix product having a last row, wherein the last row of the matrix product S<sup>−

1</sup>{overscore (K)} is explicitly set to zero; and

wherein

6. The method of claim 2 wherein combining the rate measurements of multiple rate sensing devices performed by the continuous-time steady state Kalman filter comprises:

determining N, wherein N is a number of multiple rate sensing devices being measured;

determining Q_b and R, wherein Q_b and R are N×

N covariance matrices whose elements correspond to correlation factors between the N multiple rate sensing devices;

choosing a random walk intensity q_ω

for a true rate; and

forming

$Q=\left[\begin{array}{cc}{Q}_b& 0\\ 0& q_{\mathrm{\omega <br><br>}}\end{array}\right],$

 

wherein Q is a (N+1)×

(N+1) covariance matrix;

generating L=Q^1/2H^TR<sup>−

1</sup>HQ^1/2 wherein L is a (N+1)×

(N+1) matrix, wherein H represents a N×

(N+1) matrix in a measurement equation y=Hx+v=b+1ω

+v;

wherein ω

represents a true rate along a single axis;

wherein b represents a vector of rate biases;

wherein x is a state vector x=[b^T,ω

]^T;

wherein v is a vector of ARW noises having been stacked into vector form v=[n₁₁, n₁₂, . . . , n_1N]^T; and

wherein y is a vector representing a measured signals from N gyros;

performing an eigenvalue decomposition of L ordered such that its zero eigenvalue is last wherein L=ADA^T, wherein D is a diagonal matrix and A is an orthogonal matrix;

extracting a N×

N submatrix {overscore (D)} from matrix D according to

$D=\left[\begin{array}{cc}\stackrel{\_}{D}& 0\\ 0& 0\end{array}\right]$

extracting a (N+1)×

N submatrix {overscore (A)} from matrix A according to A=[{overscore (A)}v];

generating a steady state Kalman gain K_∞

wherein

K_∞

=Q^1/2{overscore (A)}{overscore (D)}<sup>−

1/2</sup>{overscore (A)}^TQ^1/2H^TR<sup>−

1</sup>

processing the measurements from the multiple rate sensing devices according to the recursive processing filter having an output, wherein the recursive processing filter is modeled as,

$\stackrel{.}{\widehat{\mathrm{\xi <br><br>}}}=\left[\begin{array}{cc}-{\stackrel{\_}{D}}^{\frac{1}{2}}& 0\\ 0& 0\end{array}\right]<br><br>\widehat{\mathrm{\xi <br><br>}}+S^{-1}<br><br>K_{\mathrm{\infty <br><br>}}<br><br>y$$\mathrm{ <br><br>}<br><br>\widehat{\mathrm{\omega <br><br>}}=e_{N+1}^T<br><br>S<br><br>\text{\hspace{1em}<br><br>}<br><br>\widehat{\mathrm{\xi <br><br>}}$

 

wherein S=Q^1/2A;

wherein x represents the vector x=[b^T,ω

]^T;

wherein ξ

=S<sup>−

1</sup>x is a filter state;

wherein

$e_{N+1}^T=\left[0,\mathrm{\dots <br><br>}<br><br>\text{\hspace{1em}<br><br>},0,1\right]$

 

is a selection vector; and

wherein {circumflex over (ω

)} is the output of the recursive processing filter which represents the best estimate of the virtual sensed rate.

7. The method of claim 6, wherein S⁻

1K_∞

is a matrix product having a last row, wherein the last row of S<sup>−

1</sup>K_∞

is explicitly set to zero; and

wherein

8. The method of claim 3 wherein combining the rate measurements from the multiple rate sensing devices using a digital steady state Kalman filter to generate a minimum variance rate estimate comprises:

sampling N analog signals from N rate sensing devices over a period, T;

digitizing the sampled N analog signals into a digital data point for each sampling period T of each rate sensing device;

assembling the N digitized signals from the N rate sensing devices into an N-vector;

processing the N-vector by multiplying it with a matrix {overscore (K)} operator to generate a {overscore (K)} product signal;

processing the {overscore (K)} product signal by multiplying it with a matrix S<sup>−

1</sup> operator to generate a S<sup>−

1</sup> product signal;

inputting the S<sup>−

1</sup> product signal during a k^th sampling period into a first input of a summing node having two inputs;

outputting a difference of the two inputs of the summing node to a delay operator having contents;

delaying the contents of the delay operator by one sampling period, T;

feeding back a delayed output of the delay operator to a matrix operator modeled by

$\hspace{1em}<br><br>\left[\begin{array}{cc}{\mathrm{<br><br>}}^{-{\stackrel{\_}{D}}^{1/2}<br><br>T}& 0\\ 0& 1\end{array}\right]$

 

to generate a feedback signal;

inputting the feedback signal to a second one of the inputs to the summing node wherein the feedback signal is added to the S<sup>−

1</sup> product signal generated in a k+1 sampling period;

processing the contents of the delay operator for a k^th sampling period by multiplying it with a matrix S operator to generate an S product signal; and

extracting a last matrix element of the S product signal as the best estimate of the virtual sensed rate, {circumflex over (ω

)}(kT) for the k^th sampling point.

22. The method of claim 1 wherein the multiple rate sensing devices are multiple gyros producing signals and the measuring and the combining elements use the signals from the multiple gyros.

9. An apparatus for processing signals from multiple rate sensing devices each providing a rate measurement to derive a higher accuracy output signal representing a best estimate of a virtual sensed rate, comprising:

means for measuring the rate from the multiple rate sensing devices;

means for combining measured rates from the multiple rate sensing devices in a recursive processing filter, and means for determining the weights and smoothing properties of the recursive processing filter using the statistical correlations and noise characteristics of the multiple rate sensing devices.

10. The apparatus of claim 9 wherein the means for combining the measured rates from the multiple rate sensing devices comprises a continuous-time steady state Kalman filter to generate a minimum variance rate estimate.

11. The apparatus of claim 9 wherein the means for combining the measured rates from the multiple rate sensing devices comprises means for digitizing the rates of multiple rate sensing devices and means for digitally combining the rates from multiple rate sensing devices using a digital steady state Kalman filter.

12. The apparatus of claim 11 wherein the means for digitally combining the rates from the multiple rates sensing devices signals using a digital steady state Kalman filter comprises:

means for determining N, wherein N is a number of multiple rate sensing devices being measured;

means for determining Q_b and R, wherein Q_b and R are N×

N covariance matrices whose elements correspond to correlation factors between the N multiple rate sensing devices;

means for choosing a random walk intensity q_ω

for a true rate, and forming

$Q=\left[\begin{array}{cc}{Q}_h& 0\\ 0& q_{\mathrm{\omega <br><br>}}\end{array}\right],$

 

wherein Q is an (N+1)×

(N+1) covariance matrix;

means for generating L=Q^1/2H^TR<sup>−

1</sup>HQ^1/2, wherein L is an (N+1)×

(N+1) matrix, wherein H represents an N×

(N+1) matrix in a measurement equation y=Hx+v=b+1ω

+v;

wherein ω

represents a true rate along a single axis;

wherein b represents a vector of rate biases;

wherein x is a state vector x=[b^T,ω

]^T;

wherein v is a vector of angle random walk (ARW) noises that have been stacked into the vector form v=[n₁₁, n₁₂, . . . , n_1N]^T; and

wherein y is a vector representing the measured signals from N gyros;

means for performing an eigenvalue decomposition of L ordered such that its zero eigenvalue is last wherein L=ADA^T, and wherein D is a diagonal matrix and A is an orthogonal matrix;

means for extracting a N×

N submatrix {overscore (D)} from the matrix D according to

$D=\left[\begin{array}{cc}\stackrel{\_}{D}& 0\\ 0& 0\end{array}\right];<br><br>$

means for extracting a (N+1)×

N submatrix {overscore (A)} from the matrix A according to A=[{overscore (A)}v];

means for generating a steady state Kalman gain as

$\stackrel{\_}{K}=S<br><br>\left[\begin{array}{cc}{\stackrel{\_}{D}}^{-1/2}<br><br>\left(I-{\mathrm{<br><br>}}^{-{\stackrel{\_}{D}}^{\frac{1}{2}}<br><br>T}\right)& 0\\ 0& 0\end{array}\right]<br><br>S^T<br><br>H^T<br><br>R^{-1}=Q^{1/2}<br><br>\stackrel{\_}{A}<br><br>\text{\hspace{1em}<br><br>}<br><br>{\stackrel{\_}{D}}^{-1/2}<br><br>\left(I-{\mathrm{<br><br>}}^{-D^{\frac{1}{2}}<br><br>T}\right)<br><br>{\stackrel{\_}{A}}^T<br><br>Q^{1/2}<br><br>H^T<br><br>R^{-1}$

 

wherein S=Q^1/2A and wherein T is a sampling period; and

,

means for processing the rates from the multiple rate sensing devices according to the recursive processing filter, the recursive processing filter having an output and modeled as,

$\begin{array}{c}\widehat{\mathrm{\xi <br><br>}}<br><br>\left(\left(k+1\right)<br><br>T\right)=\left[\begin{array}{cc}{\mathrm{<br><br>}}^{-{\stackrel{\_}{D}}^{\frac{1}{2}}<br><br>T}& 0\\ 0& 1\end{array}\right]<br><br>\widehat{\mathrm{\xi <br><br>}}<br><br>\left(\mathrm{kT}\right)+S^{-1}<br><br>\stackrel{\_}{K}<br><br>\text{\hspace{1em}<br><br>}<br><br>y<br><br>\left(\mathrm{kT}\right)\\ \widehat{\mathrm{\omega <br><br>}}<br><br>\left(\mathrm{kT}\right)={\mathrm{<br><br>}}_{N+1}^T<br><br>S<br><br>\text{\hspace{1em}<br><br>}<br><br>\widehat{\mathrm{\xi <br><br>}}<br><br>\left(\mathrm{kT}\right)<br><br>\text{\hspace{1em}<br><br>}\end{array}$

 

wherein x represents a vector x=[b^T,ω

]^T;

wherein ξ

(kT)=S<sup>−

1</sup>x(kT) is a filter state at the k^th sampling point;

wherein

${\mathrm{<br><br>}}_{N+1}^T=\left[0,\mathrm{\dots <br><br>}<br><br>\text{\hspace{1em}<br><br>},0,1\right]$

 

is a selection vector; and

wherein {circumflex over (ω

)}(kT) is the output of the recursive filter which represents the best estimate of the virtual sensed rate for the k^th sampling point.

13. The apparatus of claim 12 wherein S⁻

1{overscore (K)} is a matrix product having a last row, wherein the last row of the matrix product S<sup>−

1</sup>{overscore (K)} is explicitly set to zero; and

wherein

14. The apparatus of claim 10 wherein the means for combining the measured rates of multiple rate sensing devices comprising a continuous-time steady state Kalman filter further comprises:

means for determining N, wherein N is a number of multiple rate sensing devices being measured;

means for determining Q_b and R, wherein Q_b and R are N×

N covariance matrices whose elements correspond to correlation factors between the N multiple rate sensing devices;

means for choosing a random walk intensity q_ω

for a true rate; and

means for forming

$Q=\left[\begin{array}{cc}{Q}_b& 0\\ 0& q_{\mathrm{\omega <br><br>}}\end{array}\right],$

 

wherein Q is a (N+1)×

(N+1) covariance matrix;

means for generating L=Q^1/2H^TR<sup>−

1</sup>HQ^1/2 wherein L is a (N+1)×

(N+1) matrix, wherein H represents a N×

(N+1) matrix in a measurement equation y=Hx+v=b+1ω

+v;

wherein ω

represents a true rate along a single axis;

wherein b represents a vector of rate biases;

wherein x is a state vector x=[b^T,ω

]^T;

wherein v is a vector of ARW noises having been stacked into vector form v=[n₁₁, n₁₂, . . . , n_1N]^T; and

wherein y is a vector representing the measured signals from N gyros;

means for performing an eigenvalue decomposition of L ordered such that its zero eigenvalue is last wherein L=ADA^T, wherein D is a diagonal matrix and A is an orthogonal matrix;

means for extracting a N×

N submatrix {overscore (D)} from matrix D according to

$D=\left[\begin{array}{cc}\stackrel{\_}{D}& 0\\ 0& 0\end{array}\right]$

means for extracting a (N+1)×

N submatrix {overscore (A)} from matrix A according to A=[{overscore (A)}v];

means for generating a steady state Kalman gain K_∞

wherein

K_∞

=Q^1/2{overscore (A)}{overscore (D)}<sup>−

1/2</sup>{overscore (A)}^TQ^1/2H^TR<sup>−

1</sup>

means for processing measured rates from the multiple rate sensing devices according to the recursive processing filter having an output, wherein the recursive processing filter is modeled as,

$\begin{array}{c}\stackrel{.}{\widehat{\mathrm{\xi <br><br>}}}=\left[\begin{array}{cc}-{\stackrel{\_}{D}}^{\frac{1}{2}}& 0\\ 0& 0\end{array}\right]<br><br>\text{\hspace{1em}<br><br>}<br><br>\widehat{\mathrm{\xi <br><br>}}+S^{-1}<br><br>K_{\mathrm{\infty <br><br>}}<br><br>y\\ \widehat{\mathrm{\omega <br><br>}}={\mathrm{<br><br>}}_{N+1}^T<br><br>S<br><br>\text{\hspace{1em}<br><br>}<br><br>\widehat{\mathrm{\xi <br><br>}}\end{array}$

 

wherein S=Q^1/2A;

wherein x represents a vector x=[b^T,ω

]^T;

wherein ξ

=S<sup>−

1</sup>x is a filter state;

wherein

${\mathrm{<br><br>}}_{N+1}^T=\left[0,\mathrm{\dots <br><br>}<br><br>\text{\hspace{1em}<br><br>},0,1\right]$

 

is a selection vector; and

wherein {circumflex over (ω

)} is the output of the recursive processing filter which represents the best estimate of the virtual sensed rate.

15. The apparatus of claim 14, wherein S⁻

1K_∞

is a matrix product having a last row, wherein the last row of S<sup>−

1</sup>K_∞

is explicitly set to zero; and

where

16. The apparatus of claim 11 wherein the means for combining the measured rates of the multiple rate sensing devices using a digital steady state Kalman filter to generate a minimum variance rate estimate further comprises:

means for sampling N analog signals from N rate sensing devices over a period, T;

means for digitizing the sampled N analog signals into a digital data point for each sampling period T of each rate sensing device;

means for assembling the N digitized signals from the N rate sensing devices into an N-vector;

means for processing the N-vector by multiplying it with a matrix {overscore (K)} operator to generate a {overscore (K)} product signal;

means for processing the {overscore (K)} product signal by multiplying it with a matrix S<sup>−

1</sup> operator to generate a S<sup>−

1</sup> product signal;

means for inputting the S<sup>−

1</sup> product signal during a kth sampling period into a first input of a summing node having two inputs;

means for outputting the difference of the two inputs of the summing node to a delay operator;

means for delaying the contents of the delay operator by one sampling period, T;

means for feeding back a delayed output of the delay operator to a matrix operator modeled by

$\left[\begin{array}{cc}-{\stackrel{\_}{D}}^{\frac{1}{2}}& 0\\ 0& 1\end{array}\right]\hspace{1em}<br><br>$

 

to generate a feedback signal;

means for inputting the feedback signal to a second one of the inputs to the summing node such that it is added to the S<sup>−

1</sup> product signal generated in a k+1 sampling period;

means for processing the contents of the delay operator for a k^th sampling period by multiplying it with a matrix S operator to generate an S product signal; and

means for extracting a last matrix element of the S product signal as the best estimate of the virtual sensed rate, ŵ

(kT) for the k^th sampling point to generate a minimum variance rate estimate.

17. The apparatus of claim 9 wherein each means comprises a firmware controlled digital processor for performing the recited function.

18. The apparatus of claim 9 wherein each means comprises a programmed portion of a digital software controlled computer for performing the recited function.

19. The apparatus of claim 9 wherein at least one of the means comprises a logic circuit for performing the recited function.

20. The apparatus of claim 9 wherein at least one of the means comprises a firmware controlled digital processor for performing the recited function.

21. The apparatus of claim 9 wherein at least one of the means comprises a programmed portion of a digital software controlled computer for performing the recited function.

23. The apparatus of claim 9 wherein the multiple rate sensing devices are multiple gyros producing signals and wherein the means for measuring and the means for combining use the signals from the multiple gyros.