Attitude change kalman filter measurement apparatus and method

[0001] 1. Field of the Invention

[0002] The present invention relates generally to a method and apparatus for correction of bias errors in a navigation system, and in particular to correction of bias errors in an inertial navigation system through the use of change in attitude measurements processed by a Kalman Filter.

[0003] 2. Description of the Related Art

[0004] Inertial navigation is based on systems first built using gyros and accelerometers located on a moving platform or gimbal, which required very complicated technical and power consuming constructions that were prone to failure. Later on, solid state solutions have been realized by using only discrete integrated electromechanical or electro-optical sensors attached directly to the vehicle or strapdown. These solid state systems have minimal moving parts, and consist of laser-gyros, mechanical accelerometers and/or integrated gyros and accelerometers manufactured using MEMS (Micro Electro-Mechanical System) technology.

[0005] Inertial navigation systems (INS) are used in a wide variety of applications, including civil and military aviation, cruise missiles, submarines and space technology. According to these areas of operation, the entire system and all components have to be very precise and reliable. As a consequence, the costs for such a system are still relatively high and the size is not yet so small that it can be used for mobile roboting, wearable computing, automotive or consumer electronics.

[0006] But navigation systems designed for these mobile applications require a very small and inexpensive implementation of such an INS. Industrial demand for low-cost sensors (in car airbag systems, for example) and recent progress in MEMS integration technology have led to sophisticated sensor products, which are now both small (single chips) and inexpensive.

[0007] A body's actual spatial behavior/movement can be described with six parameters: three translatory (x-, y-, z-acceleration) and three rotatory components (x-, y-, z-angular velocity). To be able to define the movement of the body, three acceleration sensors and three gyros have to be put together on a platform in such a way that they form an orthogonal system either physically or mathematically. The distance translated and the angle the body has actually rotated can be obtained by integration of the individual translatory and rotatory components. Performing these calculations accurately and periodically enables the INS to trace its movement and to indicate its current position, velocity, pitch, roll, and heading.

[0008] The main limitation of the system performance is due to the finite precision or accuracy of the sensors. For example, a continuous small error in acceleration will be integrated and results in a significant error in measured or predicted velocity. The velocity is then integrated a second time and will result in a position error. Therefore very precise sensors and error correction mechanisms are necessary for an accurate inertial navigation platform.

[0009] A paper published by R. E. Kalman in 1960, A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME-Journal of Basic Engineering, 82(Series K): pages 35-45(1960) described a recursive solution to the discrete-data linear filtering problem. The Kalman filter is a set of mathematical equations to provide a computational solution of the least-square method.

[0010] In his book, Stochastic Models, Estimation, and Control vol. 1, Chapter 1, pages 1-16 (1979), Peter S. Maybeck discusses the Kalman filter as an optimal linear estimator.

[0011] Greg Welch et al. review use of the Kalman filter in the area of autonomous or assisted navigation in the paper, An Introduction to the Kalman Filter, UNC-Chapel Hill, TR 95-041, Mar. 11, 2002.

[0012] The present invention provides an application of a Kalman filter to determine and remove gyro bias errors from an inertial navigation system. This implementation adds an attitude change measurement to the Kalman filter. This change may be made in the heading measurement and/or the level attitudes (pitch and roll), and provides the observability needed to estimate the gyro biases in the inertial system. For example, when the system is stationary it is undergoing zero change in heading, pitch, and roll relative to the earth. This contrasts with the conventional approach of using a known pitch, roll, and heading as the measurement for the Kalman filter.

[0013] To improve the performance of the inertial sensors, a Kalman filter is employed to estimate the inertial sensors errors using measurements from a variety of sources. Measurements are processed in the Kalman filter. For example, the fact that the INS (inertial navigation system) is stationary (at a known position and heading, and zero velocity), information from a Global Positioning System (GPS), or information from another INS can be a measurement. The processing by the Kalman filter algorithm results in an estimate of the sensor errors (e.g. bias, scale-factor, non-linearity) which is used to correct the errors.

[0014] FIG. 1 is a block diagram of the basic structure of the present system; and

[0015] FIG. 2 is functional block diagram showing the navigation system with the Kalman filter in greater detail.

[0016] FIG. 1 is a block diagram showing a navigation system 10, such as may be used in a vehicle or other moving body, which receives as inputs three signals from accelerometers 12 and three signals from gyros 14. The navigation system performs a strapdown navigation reference computation and provides as an output a navigation state signal (x) on 16. This navigation state signal is provided to a Kalman filter 18. In some applications, an aiding source 20 is connected to the Kalman filter 18 is to provide a change in attitude measurement. In other applications, the aiding source is the act of performing the measurements while the body is stationary. The Kalman filter 18 generates a signal on 20 that is returned to the navigation system 10, to thereby correct the bias errors which would otherwise cause significant errors in the navigation system's estimate of the vehicle's (or body's) movement.

[0017] FIG. 2 provides further detail of the present invention. Relevant additional details of the navigation portion 10 and of the Kalman filter portion 18 from FIG. 1 are shown separated by a broken line 22. The navigation portion 10 has the input 14 for the gyro compensated incremental angles which is provided to an adder 24, that is in turn connected to a navigation computation portion 26 where the compensation is computed using information from a factory calibration or previous usages of the INS. The adder 24 also receives an input from a multiplier unit 29. The multiplier unit 28 scale the accumulated total bias rate correction from the summing unit 28 to incremental rates. The input to the multiplier unit 29 is a summing unit 28, which represents the Kalman filter's accumulated estimate of the gyro bias. The output of the adder 28 are provided to an attitude and heading reference function unit 30 in the navigation computation portion 26. The attitutude and heading reference function unit 30 produces a signal hdg(t_k) that is forwarded to the Kalman filter portion 18.

[0018] The detail for the Kalman filter operation shown in FIG. 2 shows only the heading error correction. The preferred embodiment of the invention has similar signal paths for pitch and roll correction which are the same, but these are omitted for clarity. In the Kalman filter function, the signal hdg(t_k) is sent to two sub-functions. The first sub-function is a delay unit 32 to delay by a factor of t_(kf) to generate a signal hdg(t_(k1)). The two signals hdg(t_k) and hdg(t_(k1)) are forwarded to an adder 34, with the signal hdg(t_k) being fed to the adding input of the adder 34 and the signal hdg(t_(k1)) being fed to the subtracting input of the adder 34. The output of the adder is fed to the adding input of a further adder 36. This further adder 36 also has an input 38 connected to receive the observed heading change relative to an Earth over the time T_KF. In the case where the INS is at rest (stationary) this measurement is zero. The further adder has another subtracting input 40 which will be discussed later.

[0019] The output of the further adder 36 is provided to the Kalman filter engine 42 as a signal y_k. The Kalman filter operation will be discussed in detail hereinafter. The two outputs 44 and 46 of the Kalman filter engine 42 are an attitude (pitch and roll) and heading reset signal on 44 and a gyro bias reset rate signal on 46. The attitude and heading reset signal 44 is input to the attitude and heading function 30 and to an attitude and heading reset summer 48. The gyro bias rate resets signal 46 goes to the integrator 28. The integrator 28 integrates the gyro bias rate resets signal 46 over the time T_KF, the time period of the delay unit 32. The integrated output of the integrator 48 is fed to the subtracting input 40 of the further adder 36.

[0020] In detail, the Kalman filter state vector x is defined,

[0021] where:

[0022] ⁿthe 3-dimensional vector (or 3-vector) representing the errors in the computed sensor-to-navigation frame tranformation matrix C_sⁿ,

[0023] ^s3-vector representing the angular drift rate errors (or biases) of the three gyros in the sensor frame.

[0024] Typically, in most attitude reference and inertial navigation applications, the filter state vector is larger due to inclusion of other elements, but the six-element vector shown here suffices to describe the present invention.

[0025] The continuous time dynamical model is

[0026] where:

[0027] ⁿthe earth spin rate vector in the navigation frame,

[0028] ⁿthe transport rate vector in the navigation frame,

[0029] C_sⁿ33 direction cosine matrix defining the transformation of a vector from the sensor frame to the navigation frame,

[0030] a three vector white noise process causing a random walk in the gyro drift error vector ^s, and

[0031] _ARWa three vector white noise process causing an angle random walk attitude/heading error growth.

[0032] This can be written in partitioned form as $\left[\begin{array}{c}\underset{\_}{{\stackrel{.}{}}^n}\\ \underset{\_}{{\stackrel{.}{}}^s}\end{array}\right]=\left[\begin{array}{cc}-\left\{{\underset{\_}{}}^n+{\underset{\_}{}}^n\right\}& -C_s^n\\ 0& 0\end{array}\right]\left[\begin{array}{c}\underset{\_}{{}^n}\\ \underset{\_}{{}^s}\end{array}\right]+\left[\begin{array}{c}\underset{\_}{{}_{\mathrm{ARW}}}\\ \underset{\_}{{}_{}}\end{array}\right]$

[0033] or in more compact form as

[0034] where (in this example) F is a 66 matrix and the x and vectors are a 61 matrix. Note that the matrix $\left[\begin{array}{cc}-\left\{{\underset{\_}{}}^n+{\underset{\_}{}}^n\right\}& -C_s^n\\ 0& 0\end{array}\right]$

[0035] depicts a 66 matrix, where each quadrant is a 33 matrix. That is, 0 represents a 3 by 3 matrix containing all zeroes, and the notation defines a 33 matrix $\left[\begin{array}{ccc}0& -{}_z-{}_z& {}_y+{}_y\\ {}_z+{}_z& 0& -{}_x-{}_x\\ -{}_y-{}_y& {}_x+{}_x& 0\end{array}\right]$

[0036] The usual representation of the model is in discrete time:

[0037] where is normally approximated by a series expansion in F, such as the second order approximation:

[0038] The Kalman filter state vector is propagated between measurements as

[0039] The error covariance propagation is

[0040] where Q_kEW_kW_k^T, P_k is the covariance matrix of the error in the estimate {circumflex over (x)}_k and P₀ represents the initial uncertainty in the state vector elements.

[0041] An attitude or heading change measurement update is made wherein the Kalman filter's measurement is the difference between, on one hand, the inertially computed attitude or heading change of the KF update interval t_KF (less any attitude or heading resets applied by the filter during the interval) and, on the other hand, the externally observed attitude or heading change over the t_KF update interval.

[0042] For a non-rotating IMU, the externally observed attitude or heading change (at the aiding source) is taken to be zero. For a rotating IMU platform, the externally observed attitude or heading change would be determined using other sensors, such as a compass INS, or magnetometer, a star tracker, etc. and provided to the input 38 as shown in FIG. 2.

[0043] The following paragraph describes only the heading change measurement. The attitude change measurement is identical except that is uses pitch or roll rather than heading and uses the pitch or roll components of the attitude matrix (C) and the state vector. The change in heading measurement is

[0044] The KF measurement model is

[0045] where H_k is the 16 measurement sensitivity matrix

[0046] where {overscore (C_sⁿ)}(i, j) is the average of the i,j element

[0047] of C_sⁿ over the measurement t_KF interval, and

[0048] v_k represents the noise in the heading change difference measurement, with

[0049] The filter's measurement update of the state vector and the covariance matrix proceed in the usual fashion. One implementation being

[0050] There are many well known alternative methods in the published literature defining alternative implementations of the Kalman gain and covariance update calculations, all of which can be used in lieu of the above form, and are within the scope of the present invention.

[0051] The post measurement update state vector {circumflex over (x)}_k can be used to reset the attitude and heading direction cosine matrix C_sⁿ and the gyro drift compensation parameters.

[0052] One example of the the software implementation of the change in heading

[0053] One example of the the software implementation of the change in heading measurement to the Kalman filter is, as follows:

[0054] The Kalman filter is modified to include a change in heading measurement. Once again only the change in heading measurement is shown for simplicity. A similar implementation is used for the level attitudes. The detailed software implementation is as follows for a heading change measurement:

	// Heading change measurement
	// Zero heading change measurement should only be enabled
	when the
	// platform is stationary.
	/
	if (first_pass)
	first_pass false;
	else
	filter.kalman.measurement_formATT2
	KF_UD_UTLLITIES::CONVENTIONAL;
	heading_change error ANGLE_MATH::Plus_or_Minus_Pi
	(previous_heading_after_reset
	passed_true_heading_prior_to_reset);
	delta_time filter.time previous_time;
	if ( (delta_time < filter.delta_time *2)
	&&(filter.desired_mode SMALL_AZIMUTH))
	earth_plus_transport_rate
	filter.average.transport_rate_LV
	filter.average.earth_rate_LV;
	filter.kalman.Y.vectorATT2
	heading_change_error.angle;
	filter.kalman.R.vectorATT2
	filter.measurement_noise_stdv.ATT *
	filter.measurement noise stdv.ATT;
	filter.kalman.H.matrixATT2PSIX
	earth_plus_transport rate.vector1 * delta_time;
	filter.kalman.H.matrixATT2PSIY
	earth_plus_transport rate.vector0 * delta_time;
	filter.kalman.H.matrixATT2GBIASX
	filter.average.body_to_lv_dcm.matrix20 * delta_time;
	filter.kalman.H.matrixATT2GBIASY
	filter.average.body_to_lv_dcm.matrix21 * delta_time;
	filter.kalman.H.matrixATT2GBIASZ
	filter.average.body_to_lv_dcm.matrix22 * delta_time;
	filter.kalman.measurement_available.vectorATT 2 true;
	//
	// Increment and measurement validity data.
	//
	filter.measurement_validity.number_available
	filter.measurement_validity.number_available 1;
	filter.measurement_validity.valid_id.
	vectorfilter.measurement_validity.number_valid
	ATT2; filter.measurement_validity.number_valid
	filter.measurement_validity.number_valid 1;

[0055] The present invention permits the inertial system to estimate bias errors, thereby permitting the use of poorer performing gyros, which cost less, yet achieve the performance of a INS containing better performing gyros.

[0056] The present system can be used on a wide variety of vehicles. For example, robotic vehicles may benefit from the present system. The present system may also be used on missiles, rockets and other guided bodies. The present system augments the performance of a INS that relies on GPS (Global Positioning Satellite) systems, which are subject to being blocked by obstructions and which may be jammed whether or not the GPS is configured to provide attitude information.

[0057] Although other modifications and changes may be suggested by those skilled in the art, it is the intention of the inventors to embody within the patent warranted hereon all changes and modifications as reasonably and properly come within the scope of their contribution to the art.