Method and apparatus for adaptive filter based attitude updating

• US 20050240347A1
• Filed: 04/15/2005
• Published: 10/27/2005
• Est. Priority Date: 04/23/2004
• Status: Abandoned Application

• Current Assignee: Yun-Chun Yang
• Original Assignee: Yun-Chun Yang
• Inventors: Yang, Yun-Chun
• Application Number:
  US11/107,085
• Publication Number:
  US 20050240347A1
• Time in Patent Office: Days
• US Class Current: 701/220
• CPC Class Codes: G01C 21/183 Compensation of inertial measurements, e.g. for temperature effects

Abstract

A six state Kalman filter is adapted based on a current acceleration mode of an INS device. Gyro measurements are used to determine the acceleration mode and the Kalman filter estimates bias and small angle error of the measurements based on the acceleration mode. The bias error corrects the gyro measurement and the small angle error is used along with the corrected gyro measurement to update an attitude sensed by the gyro.

Claims

1. A method, comprising the steps of:

determining an acceleration mode;

adapting a filter with parameters matching the determined acceleration mode; and

applying the adapted filter to a correction value to determine an estimated error.

2. The method according to claim 1, wherein:

the correction value is at least one of a small angle error and bias error of an inertial navigation device.

3. The method according to claim 2, wherein the correction value is a residual of a predicted and measured value comprising at least one of a small angle error and bias error of an inertial navigation device.

4. The method according to claim 3, wherein the inertial navigation device is an Inertial Measurement Unit (IMU).

5. The method according to claim 3, wherein the filter comprises a Kalman filter.

6. The method according to claim 5, wherein the parameters comprise parameters selected to enable the Kalman filter to produce accurate estimate error determinations for the IMU under the determined acceleration.

7. The method according to claim 5, wherein the Kalman filter is configured to perform a time update and a measurement update for the determined acceleration.

8. A method, comprising the steps of:

determining an acceleration mode;

adapting a Kalman filter with parameters consistent with the determined acceleration mode; and

applying the adapted Kalman filter to a correction value to determine an estimated error of an inertial navigation device;

wherein;

the correction value comprises a difference between a measured inertia value of the inertial navigation device and a predicted inertia value;

the determined acceleration mode comprises one of a non-acceleration mode, a low acceleration mode, and a high acceleration mode; and

the parameters comprise non-acceleration Kalman filter parameters when the determined acceleration mode is the non-acceleration mode, the parameters comprise low acceleration Kalman filter parameters when the determined acceleration mode is the low acceleration mode, and the parameters comprise high acceleration Kalman filter when the determined acceleration mode is the non-acceleration mode.

9. The method according to claim 8, wherein:

the error correction value is at least one of a small angle error and bias error of an inertial navigation device.

10. The method according to claim 9, wherein the inertial navigation device is an Inertial Measurement Unit (IMU).

11. An attitude determination device, comprising:

an inertial measurement device;

an error estimator configured to produce an estimated error of the inertial measurement device; and

an attitude update device configured to update an attitude based on measurements from the inertial measurement device and the estimated error;

wherein the estimated error is based on a relationship that relates a dynamic model of error of the inertial measurement device and a measurement model of the inertial measurement device to the estimated error.

12. The method according to claim 11, wherein the dynamic model comprises a small rotation angle error vector of the inertial measurement device and a bias vector of the inertial measurement device combined to estimate the error of the inertial measurement device.

13. The attitude determination device according to claim 11, wherein the dynamic model comprises an error state model comprising a Kalman filter adapted for multiple operating conditions.

14. The attitude determination device according to claim 13, wherein the Kalman filter is adapted for non-acceleration, low acceleration, and high acceleration operating conditions.

15. The attitude determination device according to claim 12, wherein the Kalman filter is configured to implement the dynamic model and the measurement model.

16. The attitude determination device according to claim 15, wherein the measurement model comprises an accelerometer measurement and a magnetic compass measurement.

17. The attitude determination device according to claim 15, wherein the dynamic model is based on

$\delta$

⁢

 

⁢

x = [ δ

ρ

x g ]

with δ

ρ

=[ε

_N,ε

_E,ε

_D]^T being a small rotation angle error vector of the gyroscope, and x_g=[g_x,g_y,g_z]^T being a bias vector of the gyroscope.

18. The attitude determination device according to claim 11, wherein the error estimator includes a state model comprising

$\delta$

⁢

 

⁢

x = [ δ

ρ

x g ]

with δ

ρ

=[ε

_N,ε

_E,ε

_D]^T being a small rotation angle error vector of the gyroscope, and x_g=[g_x,g_y,g_z]^T being a bias vector of the gyroscope.

19. The attitude determination device according to claim 18, wherein the error state model is a dynamic error state model comprising:

$\left[\begin{array}{c}\mathrm{\delta <br><br>}<br><br>\stackrel{.}{\mathrm{\rho <br><br>}}\\ {\stackrel{.}{x}}_g\end{array}\right]=\left[\begin{array}{cc}{F}_{\mathrm{\rho <br><br>\rho <br><br>}}& F_{\mathrm{\rho <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>x_g}\\ 0& F_{x_g<br><br>x_g}\end{array}\right]<br><br>\left[\begin{array}{c}\mathrm{\delta <br><br>\rho <br><br>}\\ x_g\end{array}\right]+\left[\begin{array}{c}{\mathrm{\omega <br><br>}}_{\mathrm{\rho <br><br>}}+{\mathrm{\upsilon <br><br>}}_g\\ {\mathrm{\omega <br><br>}}_g\end{array}\right],\mathrm{where}$ $F_{\mathrm{\rho <br><br>\rho <br><br>}}=\left[\begin{array}{ccc}0& {\mathrm{\omega <br><br>}}_D& -{\mathrm{\omega <br><br>}}_E\\ -{\mathrm{\omega <br><br>}}_D& 0& {\mathrm{\omega <br><br>}}_N\\ {\mathrm{\omega <br><br>}}_E& -{\mathrm{\omega <br><br>}}_N& 0\end{array}\right];<br><br>$ $F_{\mathrm{\rho <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>X_g}=\frac{\partial <br><br>\mathrm{\rho <br><br>}}{\partial <br><br>{\mathrm{\omega <br><br>}}_{\mathrm{ib}}^b}<br><br>\frac{\partial <br><br>{\mathrm{\omega <br><br>}}_{\mathrm{ib}}^b}{\partial <br><br>x_g}=R_{b<br><br>\text{\hspace{1em}<br><br>}<br><br>2<br><br>t}<br><br>\frac{\partial <br><br>{\mathrm{\omega <br><br>}}_{\mathrm{ib}}^b}{\partial <br><br>x_g}=R_{b<br><br>\text{\hspace{1em}<br><br>}<br><br>2<br><br>t};<br><br>\text{\hspace{1em}<br><br>}<br><br>F_{x_g<br><br>x_g}=0;<br><br>$ ${\mathrm{\omega <br><br>}}_N={\mathrm{\omega <br><br>}}_{\mathrm{ie}}<br><br>\cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\lambda <br><br>}+\frac{{\mathrm{\upsilon <br><br>}}_E}{R_{\mathrm{\Phi <br><br>}}+h};<br><br>\text{\hspace{1em}<br><br>}<br><br>{\mathrm{\omega <br><br>}}_E=\frac{{\mathrm{\upsilon <br><br>}}_N}{R_{\mathrm{\lambda <br><br>}}+h};<br><br>\mathrm{and}$ ${\mathrm{\omega <br><br>}}_D={\mathrm{\omega <br><br>}}_{\mathrm{ie}}<br><br>\sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\lambda <br><br>}-\frac{\tan <br><br>\left(\mathrm{\lambda <br><br>}\right)<br><br>{\mathrm{\upsilon <br><br>}}_E}{R_{\mathrm{\Phi <br><br>}}+h}.$

20. The attitude determination device according to claim 19, wherein x_g is modeled as a random walk process with F_xgxg being 0.

21. A method, comprising the steps of:

measuring at least one inertia based force acting on a body;

identifying a set of parameters that define operation of a characteristic model under the measured inertia;

applying the set of parameters to the model; and

determining the characteristic under the measured inertia from the model.

22. The method according to claim 21, wherein the model is a Kalman filter that is adaptable to plural sets of parameters, each set of parameters defining operation of the Kalman filter for a predetermined range of inertial forces.

23. The method according to claim 21, wherein the model is an error model of an inertial measurement device.

24. The method according to claim 23, wherein the inertial measurement device is a gyroscope.

25. The method according to claim 23, wherein the inertial measurement device is a 3-axis gyroscope.

26. The method according to claim 23, wherein the inertial measurement device is a MEMS based Inertial Measurement Unit (IMU).

27. The method according to claim 21, wherein:

the model is a Kalman filter that is adaptable to plural sets of parameters, each set of parameters defining operation of the Kalman filter for a predetermined range of inertial forces;

the characteristic model is an estimated error model for each axis of a 3 axis gyroscope; and

said step of measuring comprises measuring the inertial force in each axis of the 3-axis gyroscope.

28. The method according to claim 27, further comprising the step of applying the estimated error from the model to an attitude determination device to compensate for error.

29. The method according to claim 27, further comprising the step of applying the estimated error from the model to a quaternion based attitude update process.

30. The method according to claim 27, further comprising the steps of:

determining an initial attitude of the body; and

updating the attitude of the body taking into account the estimated error from the model.

31. The method according to claim 30, wherein the step of updating the attitude comprises updating the attitude as a strapdown INS.

32. The method according to claim 31, wherein the strapdown INS attitude update comprises a quaternion based attitude update.

33. The method according to claim 32, wherein the quaternion based attitude update comprises a quaternion propagation comprising the steps of,

integrating a differential equation relating the quaternion (q) and an angle rate for quaternion propagation to determine the quaternion;

normalizing the quaternion; and

rotating a frame of the body to a tangent frame representative of the quaternion propagation.

34. The method according to claim 33, wherein the differential equation comprises:

$\stackrel{.}{q}=\frac{1}{2}<br><br>\left[\begin{array}{cccc}0& r& -q& p\\ -r& 0& p& q\\ q& -p& 0& r\\ -p& -q& -r& 0\end{array}\right]<br><br>\text{\hspace{1em}<br><br>}<br><br>q=\left[\begin{array}{ccc}{q}_4& -q_3& q_2\\ q_3& q_4& -q_1\\ -q_2& q_1& q_4\\ -q_1& -q_2& -q_3\end{array}\right]<br><br>\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$

where gyro measurements are denoted as ω

_ib^b=[p,q,r]^T with p, q, and r being three-axis angle rate in the body frame.

35. The method according to claim 33, wherein normalization of the quaternion comprises:

$q_n=\frac{q}{q^T<br><br>q}$

where

36. The method according to claim 33, wherein the step of rotating comprises the step of calculating a rotation matrix from the body frame to the tangent frame R_b2t such that:

R_b2t=(q₄²−

p^Tp)I<sub>3×

3</sub>+2pp^T−

2q₄[p×

] 

 

(12)

with p=[q₁,q₂,q₃]^T, I<sub>3×

3</sub> being the identity matrix, and

$\left[\mathrm{px}\right]=\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right].$

38. A six state dynamic model, comprising:

an input mechanism configured to retrieve small angle and bias information;

a dynamic model comprising a 3 axis small angle rotation vector and a 3-axis bias error defined as

$\mathrm{\delta <br><br>}<br><br>\text{\hspace{1em}<br><br>}<br><br>x=\left[\begin{array}{c}\mathrm{\delta <br><br>\rho <br><br>}\\ x_g\end{array}\right],$

wherein δ

ρ

is the 3-axis small angle rotation error vector defined as δ

ρ

=[ε

_N,ε

_E,ε

_D]^T and x_g is the 3-axis bias error being the small rotation angle error vector, and x_g=[g_x,g_y,g_z]^T; and

a processing device configured to calculate the model to produce δ

x.

37. The method according to claim 38, further comprising the step of recalculating q as:

$q_4=\pm <br><br>\frac{1}{2}<br><br>{\left(1+R_{11}+R_{22}+R_{33}\right)}^{0.5}$ $q_1=\frac{1}{4<br><br>q_4}<br><br>\left(R_{23}-R_{32}\right);<br><br>$ $q_2=\frac{1}{4<br><br>q_4}<br><br>\left(R_{31}-R_{13}\right);<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{and}$ $q_3=\frac{1}{4<br><br>q_4}<br><br>\left(R_{12}-R_{21}\right).$

39. The six state dynamic model according to claim 38, further comprising a Kalman filter, wherein the six state dynamic model is fitted to the Kalman filter, and the Kalman filter is configured to be adapted to each of a series of acceleration modes.

40. The six state dynamic model according to claim 39, further comprising a measurement model fitted to the Kalman filter.

41. The six state model according to claim 40, wherein:

the Kalman filter is configured to be adapted to each of,

a non acceleration mode, such that, when data processed by the Kalman filter from a non-accelerating inertial measurement device, the Kalman filter produces a non-acceleration error estimate of the inertial measurement device,

a low acceleration mode, such that, when data processed by the Kalman filter from a low-accelerating inertial measurement device, the Kalman filter produces a low-acceleration error estimate of the inertial measurement device, and

a high acceleration mode, such that, when data processed by the Kalman filter from a high-accelerating inertial measurement device, the Kalman filter produces a high-acceleration error estimate of the inertial measurement device,

42. An adaptive filter, comprising a set of states for estimating errors;

a time transition matrix for updating the states; and

an adaptive update mechanism configured to adapt operation of the time transition matrix based on an operational mode of the adaptive filter.

43. The adaptive filter according to claim 42, wherein the adaptive filter is a six state filter comprising 3 tilt angle states and 3 bias error states each corresponding to an axis of a 3-axis gyro.

44. The adaptive filter according to claim 42, wherein the time transition matrix comprises a Kalman filter adaptable to each of the operational modes.

45. The adaptive filter according to claim 42, wherein the operational modes comprise a non accelerator mode, a low dynamic mode, and a high dynamic mode.

46. The adaptive filter according to claim 43, wherein the gyroscope comprises a MEMS based Inertial Measurement Unit (IMU).

47. The adaptive filter according to claim 42, wherein further comprising an accelerometer and a compass configured to update the time transition matrix.

48. An adaptive filter configured to determine gyro and bias errors for use in an inertial device comprising 3 gyroscopes wherein the adaptive filter has at least six states comprising three tilt angles detected by the gyroscopes and a bias error for each gyroscope and the adaptive filter includes at least three adaptive states including a no acceleration mode, a low acceleration mode, and a high acceleration mode.

49. The adaptive filter according to claim 48, wherein a current adaptive state of the adaptive filter is determined by an analysis of accelerometer data.

50. The adaptive filter according to claim 48, wherein the adaptive filter is implemented on a processing board of the inertial device within an industry standard INS device enclosure.

51. The adaptive filter according to claim 50, wherein the gyroscopes are integrated into a single 3 axis MEMS based device.