System and method for estimating sensor errors

[0002] This invention relates to a system and a method for estimating sensor errors and more particularly, to a system and a method which accurately estimates errors within an integrated navigation system, such as scale factor and bias errors.

[0003] Integrated navigation systems (“INS”) are employed within vehicles in order to provide vehicle position and velocity information with respect to a specified reference frame. A typical INS determines estimates for the position and velocity of a vehicle based on a collection of data taken from inertial sensors such as acceleration and rate sensors mounted in the vehicle, as well as sensors based outside the vehicle such as a global position system (“GPS”). Typically, the INS will use this sensor information, along with a model of vehicle motion behavior to form a set of navigation equations, in order to estimate vehicle position information and derivatives. Conventional INS may be used in “turn-by-turn navigation” systems, within “in vehicle dynamics” systems, and within proposed vehicle enhancements such as “adaptive cruise control.”

[0004] A key element and/or function of the INS is the estimation of sensor errors used in the navigation equations. All sensors used by the INS have a scale factor that relates the sensor output to the sensed attribute, and a bias error (i.e., the sensor has a nonzero output even when the sensed attribute is zero). If the bias error or scale factor estimates are calculated incorrectly, the calculated vehicle position, heading and/or speed will be in error, and the reliability of the INS will be undesirably reduced. This sensor error estimation is especially important in situations where data from the GPS may become unavailable (e.g., under bridges or within tunnels).

[0005] Efforts have been made to reduce the impact of the scale factor and bias error estimation through the use of high quality inertial measurement equipment. However, the relatively high cost of such equipment is prohibitive for automotive applications. Hence, sensor error estimation is critical in conventional systems utilizing lower quality sensors.

[0006] Conventional sensor error estimation is typically performed by developing sensor error models, and then implementing the model parameter estimation as augmented equations in the overall set of navigation equations, usually in a “Kalman” filter. The Kalman filter approach has desirable stability properties, but is somewhat limited, as it provides only statistical representations of errors, and requires the implementation to be added to the overall navigation equations. Other attempts at estimating these types of sensor errors have been made using neural networks. While neural networks have the advantage of learning in the presence of noise, they often require a relatively large number of learning examples (e.g., a training set) which are needed for the training phase. The required “training” process is relatively complicated, time consuming and computationally intensive, and is therefore not suited to be carried out “on-line” or during the normal use of a vehicle.

[0007] Applicants' invention addresses these drawbacks and provides a method for accurately estimating sensor errors within an integrated navigation system such as scale factor and sensor bias errors.

[0008] It is a first object of the invention to provide a system and a method for estimating sensor errors which overcomes at least some of the previously delineated drawbacks of prior systems, devices, and/or methods.

[0009] It is a second object of the invention to provide a system and a method for estimating sensor errors which is adapted for use within an integrated navigation system.

[0010] It is a third object of the invention to provide a system and a method for estimating sensor errors by integrating an INS and a GPS using a linear neuron. The linear neuron adaptively estimates scale factor and bias errors in a yaw rate sensor during the availability of the GPS, and then uses these estimated values to aid the INS during periods of time in which the GPS is unavailable or unsuitable for use. The linear neuron used by the present system and method avoids parameter identification disadvantages and extensive training requirements inherent in neural networks. The present system and method further reduce problems associated with persistent excitation inherent in prior “Kalman” filter type estimations.

[0011] According to one aspect of the present invention, a system is provided for estimating errors in a first sensor. The system includes a module which is communicatively coupled to the first sensor and to a second sensor, and which includes a first portion which selectively generates a first value based upon a signal received from the first sensor, a second portion which selectively generates a second value based upon a signal from the second sensor, and a third portion which combines the first and second value, effective to generate a third value. The system further includes a linear neuron which is communicatively coupled to the module and which receives the first value and the third value, and which estimates the errors within the first sensor based upon the received first and third values.

[0012] According to a second aspect of the present invention, a system is provided for estimating a scale factor and bias error of a yaw rate sensor which is used within an integrated navigation system having a global position system sensor. The system includes a first portion which receives a first signal from the global position system sensor and calculates a global position system heading angle based upon the first signal; a second portion which receives an integrated navigation system heading angle; a third portion which combines the global position system heading angle and the integrated navigation system heading angle, effective to generate a combined heading angle; and a linear neuron which is communicatively coupled to the third portion, which receives the integrated navigation system heading angle and the combined heading angle and which is effective to accurately estimate the scale factor and the bias error based upon the received integrated navigation system heading angle and combined heading angle.

[0013] Further objects, features, and advantages of the invention will become apparent from the following detailed description of the preferred embodiment of the invention and by reference to the following drawings.

[0014] FIG. 1 is a block diagram of a system for estimating sensor errors which is made in accordance with the teachings of the preferred embodiment of the invention.

[0015] FIG. 2 is a graph illustrating a determination and/or calculation of a DGPS heading angle by using position signal data received from a DPGS sensor.

[0016] FIG. 3 is a graph illustrating the derivation of geometry-based heading angle statistics using position data.

[0017] FIG. 4 is a diagram illustrating a distance “DA” which is equal to the absolute value of the difference between an INS and a DGPS heading angle.

[0018] FIG. 5 is a graph illustrating the derivation of a quantity C_kwhich is used within the equations delineated below.

[0019] FIG. 6 is a block diagram illustrating one embodiment of a linear neuron design used by the system shown in FIG. 1.

[0020] FIG. 7. is a graph illustrating the average bias estimates provided by the system shown in FIG. 1 over a period of time.

[0021] FIG. 8 is a graph illustrating the weighted scale factor estimates provided by the system shown in FIG. 1 over a period of time.

[0022] FIG. 9 is a graph illustrating the average bias estimates provided by a Kalman filter type system over a period of time.

[0023] FIG. 10 is a graph illustrating the scale factor estimates provided by a Kalman filter type system over a period of time.

[0024] FIG. 11 is a graph illustrating the “gap-filling” function of the system shown in FIG. 1 in comparison to the “gap-filling” function provided by a Kalman filter type system.

[0025] Referring now to FIG. 1, there is shown a block diagram of a system 10 for estimating sensor errors which is made in accordance with the teachings of the preferred embodiment of the invention. In the preferred embodiment of the invention, system 10 is utilized by a vehicle integrated navigation system (“INS”) to estimate sensor errors (i.e., scale factor and bias error of a yaw rate sensor) and to more accurately predict the error in the heading angle determined by the INS. In the preferred embodiment of the invention, system 10 comprises a portion of the entire INS. In alternate embodiments, system 10 can be used to accurately estimate and/or determine errors in other types of sensors and systems which measure different types of attributes and generate signals corresponding to the measured attributes.

[0026] As shown, system 10 includes a system module 12 and a linear neuron 14 which is communicatively coupled to the module 12 in a conventional manner. As should also be apparent to those of ordinary skill in the art, module 12 may include one or more memory units and may actually comprise a plurality of commercially available, conventional, and disparate chips or devices, which are operatively and communicatively linked in a cooperative manner. As shown in FIG. 1, module 12 includes several portions 16, 18, 20, 22 and 24 which perform functions related to the operation of the preferred embodiment of the present invention, as described more fully and completely below.

[0027] System module 12 is electrically, physically, and communicatively coupled to various sensors (not shown) which provide input signals to module 12 and/or to other portions of the INS. For example and without limitation, module 12 receives a digital global position system (“DGPS”) signal from a DGPS sensor (not shown) which provides and/or is used to calculate a DPGS heading angle “ψ_dgpsk”; and a yaw rate signal “r” from a conventional vehicle yaw rate sensor (not shown). Module 12 further receives and/or calculates the standard deviation of the DPGS measured position “σ_Pdgps”.

[0028] As described more fully and completely below, module 12 “smoothes” the DGPS heading angle ψ_dgpskby use of a simple variable-coefficient filter. The smoothed DGPS heading angle ψ_dgpskis then combined with the INS heading angle “ψ_ins”, which is provided and/or determined in a conventional manner, based on the statistics of both angles and the angle differences. The combined angle “ψ_f” is then communicated or “fed” to the linear neuron 14 along with open loop INS angle ψ_insin order to estimate the scale factor “δ_k” and the bias “B” of the yaw rate sensor. The estimated scale factor δ_kand bias B are the communicated or “fed back” into module 12 in order to generate the statistics of the INS heading angle ψ_insfor the next time sample.

[0029] Using the linear neuron 14 to estimate the bias and the scale factor in the yaw rate sensor requires a desired target of the heading angle error with an acceptable accuracy. The following description discloses a non-limiting method of obtaining a good estimate of the heading angle from both the received DGPS positions and the INS heading angle.

[0030] The first step in the implementation of the proposed system is to obtain the DGPS heading angle ψ_dgpsk, and then smooth it in order to overcome “jittering” and “outlier” effects. In the preferred embodiment of the invention, a conventional variable-coefficient filter or “smoother” 16 is applied to the DGPS signal for this purpose. Referring now to FIG. 2, there is shown a graph 30 illustrating the determination and/or calculation of the DGPS heading angle ψ_k(e.g., ψ_dgpsk) by using position signal data received from the DPGS sensor at events k and k−1 (e.g., positions P_gkand P_gk−1).

[0031] The heading angle ψ_k(e.g., ψ_dgpsk), measured clockwise from the “Y” axis, at time t_kcan be written as

[0032] ψ_k=atan 2(y_gk−y_gk−1, x_gk−x_gk−1),  (Eq. 1)

[0033] where atan2 is the four quadrant inverse tangent. The “smoothed” average heading angle ψ_ks, at time t_kis the weighted average of the heading angles at times t_kand t_k−1, which may be represented by the following equation: $\begin{array}{cc}{\psi }_{k_s}=\left(\frac{\frac{W_1}{1+\left|S\right|}{\psi }_{k-1_s}+W_2{\psi }_k}{\frac{W_1}{1+\left|S\right|}+W_2}\right)& \left(\mathrm{Eq}.2\right)\end{array}$

[0034] where S is the sum of the yaw rates between the current and last DGPS positions. In this non-limiting embodiment, the yaw rate is being measured at a faster rate than the DGPS measurement rate. Furthermore, in this non-limiting example, W₂is selected to be greater than W₁to give more weight to ψ_k, and as |S| increases, even less weight is assigned to ψ_k−1. In the preferred embodiment of the invention, the calculated heading angle ψ_kis required to pass a “smoothness test” before it is used within Eq. 2. If the calculated heading angle ψ_kfails to pass the smoothness test, the filter begins a new phase of the filtering process.

[0035] The smoothness test is based on the value of S and the expected range of the yaw rate measurements. That is, the change in the DGPS-measured heading angle between the k and k−1 DGPS positions (i.e., ψ_k−ψ_k−1) should be relatively close in value to the accurate value of S if P_kis not an outlier with respect to the DGPS filtered path. Hence, if S is greater than the expected range of its error, the sign of (ψ_k−ψ_k−1) should be the same as the sign of S. Therefore, setting the INS sample period to dt_iand the DGPS sample period to dt_g, ψ_kshould satisfy the following equation:

[0036] ψ_k−1s+Sdt_i−2*Δ≦ψ_k≦ψ_k−1s+Sdt_i+2*Δ  (Eq. 3)

[0037] where Δ is the expected net error of the yaw rate measurements over the time period dt_g. It should be appreciated that by using the “2*Δ” term, any possible error in this net expected value should be overcome.

[0038] Module 12 utilizes a statistics generator and combiner portion 18 to derive statistics for the DGPS heading angle ψ_dgpskwhich are necessary to statistically combine the DGPS heading angle with the INS heading angle ψ_ins. Given the k and k−1 position statistics and the radial distance between the two positions, geometry-based heading angle statistics can be derived, as shown by the graph 40 in FIG. 3.

[0039] The position covariances of the (x_k,y_k) coordinate are c_xxkand c_yyk. These covariances are available with the DGPS position solution, using

[0040] σ_xk={square root over (C  (Eq. 4)

[0041] σ_yk={square root over (C  (Eq. 5)

[0042] From the right triangle in graph 40 of FIG. 3, the maximum magnitude of the DGPS heading angle error E_ψ is equal to $\begin{array}{cc}{E}_{\psi }=\mathrm{atan}\left(\frac{C_k+C_{k-1}}{D}\right)& \left(\mathrm{Eq}.6\right)\end{array}$

[0043] where D is the radial distance between the k−1 and k positions. This value E_ψ is to be considered the standard deviation (σ) of the DGPS heading angle error. The quantity C_kin Eq. 6 can be calculated from the geometry shown in graph 50 of FIG. 5. Particularly, the equation of the heading line is

[0044] y_h=tan(ψ_k)x_h  (Eq. 7)

[0045] Therefore, the equation of the orthogonal to the heading line is $\begin{array}{cc}{y}_{\mathrm{oh}}=-\frac{1}{\tan \left({\psi }_k\right)}{x}_{\mathrm{oh}}& \left(\mathrm{Eq}.8\right)\end{array}$

[0046] and the ellipse equation is $\begin{array}{cc}\frac{x^2}{C_{\mathrm{xx}}}+\frac{y^2}{C_{\mathrm{yy}}}=1& \left(\mathrm{Eq}.9\right)\end{array}$

[0047] To find the coordinates of the point Q (the intersection of line OQ with the ellipse), y is equated to y_hfrom the above equation, i.e., $\begin{array}{cc}{y}^2=C_{\mathrm{yy}}\left(1-\frac{x^2}{C_{\mathrm{xx}}}\right)=\frac{1}{{\tan }^2\left(x\right)}{x}^2& \left(\mathrm{Eq}.10\right)\\ \mathrm{Thus},& \\ y=\sqrt{\frac{C_{{\mathrm{xx}}_k}{C}_{{\mathrm{yy}}_k}}{C_{{\mathrm{xx}}_k}+C_{{\mathrm{yy}}_k}{\tan }^2{\psi }_k}}& \left(\mathrm{Eq}.11\right)\\ x=\sqrt{\frac{C_{{\mathrm{xx}}_k}{C}_{{\mathrm{yy}}_k}{\tan }^2{\psi }_k}{C_{{\mathrm{xx}}_k}+C_{{\mathrm{yy}}_k}{\tan }^2{\psi }_k}}& \left(\mathrm{Eq}.12\right)\end{array}$

[0048] and thus, the distance C_kis the distance between the point Q and the point O, or $\begin{array}{cc}{C}_k=\sqrt{\frac{C_{{\mathrm{xx}}_k}{C}_{{\mathrm{yy}}_k}=\left(1+{\tan }^2{\psi }_k\right)}{C_{{\mathrm{xx}}_k}+C_{{\mathrm{yy}}_k}{\tan }^2{\psi }_k}}& \left(\mathrm{Eq}.13\right)\end{array}$

[0049] Once a “smoothed” DGPS heading angle and its statistics are obtained, the DGPS heading angle is “combined” with the INS heading angle as shown by block 20 of module 12. A geometric approach is utilized to integrate the DGPS heading angle and the INS heading angle. Particularly, the approach is based on the difference (e.g. “distance”) between the two angles and on the statistics of both angles.

[0050] Referring now to FIG. 4, this distance “DA” is illustrated in diagram 60. The distance DA is defined as

[0051] DA=|ψ_ins−ψ_dgps|  (Eq. 14)

[0052] If DA is greater the sum of the standard deviation “σ” values of both the INS heading angle and the DGPS heading angle, the ranges of the two angles will not overlap. In this case the combined heading angle will be equal to the weighted average of both angles. Defining the weights to be the reciprocals of the σ values, then $\begin{array}{cc}{W}_{\mathrm{ins}}\equiv \frac{1}{{\sigma }_{\psi \mathrm{ins}}}& \left(\mathrm{Eq}.15\right)\\ W_{\mathrm{dgps}}\equiv \frac{1}{{\sigma }_{\psi \mathrm{dgps}}}& \left(\mathrm{Eq}.16\right)\end{array}$

[0053] Therefore, the combined heading angle ψ_fis equal to the weighted average of the INS heading angle and the DGPS heading angle, i.e., $\begin{array}{cc}{\psi }_f=\frac{w_{\mathrm{ins}}{W}_{\mathrm{ins}}+w_{\mathrm{dgps}}{\psi }_{\mathrm{dgps}}}{w_{\mathrm{ins}}+w_{\mathrm{dgps}}}& \left(\mathrm{Eq}.17\right)\end{array}$

[0054] After obtaining the desired or “combined” angle ψ_fin Eq. 17, the angle is inputted into the linear neuron 14, as shown in FIG. 1 and is used to train the linear neuron 14.

[0055] To design the linear neuron 14 such that its coefficients will reflect the estimates of the bias and the scale factor, a heading error model and a desired target are determined. For the heading error model, the calculated yaw rate is assumed to be in the form

[0056] r_c=(K_a+δK)V_a+B_r+n_r,  (Eq. 18)

[0057] where r_cis the calculated yaw rate, K_ais the accurate scale factor, δK is the error in the nominal scale factor (K_A+δK), V_a(volts) is the correct direct voltage obtained from the yaw rate sensor, B_ris the bias in the calculated yaw rate (rad/sec), and n_ris the noise in the calculated yaw rate (rad/sec). Using the correct yaw rate

[0058] r_a=K_aV_a  (Eq. 19)

[0059] Eq. 18 can be written as

[0060] δr=r_c−r_a=δK_rr_a+B_r+n_r,  (Eq. 20)

[0061] where δK_ris defined as $\begin{array}{cc}\delta K_r=\frac{\delta K}{K_a}& \left(\mathrm{Eq}.21\right)\end{array}$

[0062] Eq. 20 is the error in one yaw rate sample, and is in terms of the correct yaw rate, which is “unknown”. The equation Eq. 20 can then be written in terms of the calculated yaw rate as follows

[0063] δr=δK_cr_c+B_rc+n_c,  (Eq. 22)

[0064] where $\begin{array}{cc}\delta K_c=\frac{\delta K_r}{1+\delta K_r}& \left(\mathrm{Eq}.23\right)\\ B_c=\frac{B_r}{1+\delta K_r}& \left(\mathrm{Eq}.24\right)\\ n_c=\frac{n_r}{1+\delta K_r}& \left(\mathrm{Eq}.25\right)\end{array}$

[0065] To develop the total error added to the heading angle between the k−1 and k DGPS samples (Δ_ψk−1,k), the following definition is made $\begin{array}{cc}L\equiv \frac{d{t}_g}{d{t}_i}& \left(\mathrm{Eq}.26\right)\end{array}$

[0066] and then Δ_ψk−1,kwill be the discrete integration of the L error samples, i.e.,

[0067] Δ_ψk−1,k=δr₁dt₁+δr₂dt_i+. . . +δr_Ldt_i=δK_CK(r_cl+r_c2+. . . +r_cL)dt₁+B_ckLdt_i+

[0068] (n_cl+n_c2+. . . n_cL)dt_i=S_rk−1,kδk_ckdt_i+B_ckdt_g+S_nk−1,kdt_i  (Eq. 27)

[0069] S_rk−1,kand S_nk−1,kin Eq. 27 are the respective sum of the yaw rate measurements and noises between the last and the current DGPS points, and dt_gis equal to Ldt_i. In one non-limiting embodiment, the discreet integration is performed by subroutine 22. The error in the heading angle at time k (ψ_Ek) can, therefore, be written as

[0070] ψ_E=ψ_E+(δK_cS_r+. . . δK_cS_r)dt₁₊₍B_c+. . . +B_c)dt_g+S_ndt_i  (Eq. 28)

[0071] Under the assumption that the scale factor and the bias are constant for a period of time T between an m (m<k) sample and the current sample k (e.g., T_dgps), Eq. 28 can be written as

[0072] ψ_E+ψ_E+δK_cS_rdt_i+B_cT+S_ndt_i  (Eq. 29)

[0073] If this assumption does not hold, the scale factor and the bias in Eq. 29 are set equal to the weighted average scale factor and the average bias, respectively, over the time period T.

[0074] In the preferred embodiment of the invention, the design of the linear neuron is based on Eq. 29 and may be affected by several different factors. For example and without limitation, the quality of the yaw rate sensor may affect the neuron design (e.g., the higher the quality of the yaw rate sensor, the bigger the selected value of T). This allows a sufficient amount of error to accumulate between the k−1 and k DGPS samples. These factors can be adjusted and/or modified by a designer by use of model generator portion 24 of module 12.

[0075] Since the goal of this system 10 is to estimate features (i.e., scale factor and bias), the open loop INS heading angle ψ_insis used as the angle with the accumulated error. This causes the estimator to be less affected by the quality of the combined heading angle (desired target) in Eq. 17.

[0076] Referring now to FIG. 6, there is shown one non-limiting design of the linear neuron 14. The desired target is the open loop INS heading angle ψ_insminus the combined heading angle ψ_fin Eq. 17 which represents the error in the open loop heading angle. The neuron design is simply a simulation of Eq. 29, where the neuron weights are the estimated scale factor δK and bias B, and the neuron inputs are the yaw rate sum and time. A bias input of value one is needed if T is the time calculated from zero. This is necessary to estimate the initial error in the heading angle. If the noise is zero mean, it will be smoothed out by a summing operation. The designer can “inject” the neuron with a noise level that matches what is in the yaw rate sensor specification sheet. The estimated scale factor and bias is fed back into module 12 in order to predict the error of the INS heading angle for the next DGPS period.

[0077] The present invention dynamically integrates DGPS and INS by use of a linear neuron, thereby effectively and automatically eliminating errors in vehicle heading and speed measurements. The system 10 utilizes linear neuron 14 to adaptively estimate scale factor and the bias values during the availability of the GPS, and then uses these estimated values to aid the INS during periods of time in which the GPS is unavailable or unsuitable for use. The linear neuron used by the present system and method avoids parameter identification disadvantages and extensive training requirements inherent in neural networks. The present system and method further reduce problems associated with persistent excitation inherent in prior “Kalman” filter type estimations.

[0078] Experimental results of the present system 10 demonstrate the advantages of the present system over the Kalman filter method in terms of estimation, efficiency and computation time. The graphs 70-110 of FIGS. 7-11 contain experimental data which was obtained over a segment of highway which included straight sections, curved sections, and overhead bridges. The overhead bridges were effective to block the satellite signals and make the DGPS unavailable while the vehicle was underneath the bridges.

[0079] Referring now to FIGS. 7 and 8, there are respectively shown a graph 70 illustrating the average bias estimate provided by system 10 over a period of time, and a graph 80 illustrating the weighted scale factor estimates provided by system 10 over a period of time. The average bias estimates shown in graph 70 are equal to the remaining bias after subtracting the “turn on” bias.

[0080] Referring now to FIGS. 9 and 10, there are respectively shown a graph 90 illustrating the average bias estimate provided by a system utilizing a “Kalman” filter, and a graph 100 illustrating the scale factor estimates provided by a system utilizing a “Kalman” filter. As should be appreciated by one of ordinary skill in the art, unlike system 10, the “Kalman” filter system shown in graphs 90 and 100 is unable to correctly estimate both the bias and the scale factor.

[0081] Referring now to FIG. 11, there is shown a graph 110 which illustrates the “gap-filling” function of the present system 10 in comparison to the “gap-filling” function provided by a Kalman filter type system. Particularly, graph 100 illustrates the performance of the present system 10 and a Kalman filter type system during and after a “gap” (e.g., a period of time where DGPS is unavailable) which is induced by an typical overhead bridge. As shown by graph 100, at the exit of the gap, the lateral distance between the present system 10 and the Kalman filter type system is approximately 5.5 meters. The present system and the Kalman filter type system are, respectively, 0.5 meters and 5 meters away from the DGPS solution. Additionally, the Kalman filter solution is “pulled” toward the DGPS solution by about 2 meters.

[0082] It is understood that the various inventions are not limited to the exact construction and method illustrated and described above, but that these previously delineated inventions may be varied without departing from the scope of the inventions as described in the following claims.