JP2011064484A - Sensor bias estimation device - Google Patents

Abstract

A sensor bias estimation apparatus capable of calculating a bias vector estimated value with high accuracy is obtained.
Each sensor calculates a correlation observation value based on an observation value and a predicted value, calculates a sensor wake based on the correlation observation value, and is applied to a device that outputs a calculation result. A sensor bias estimation device for estimating the bias of the sensors 10 and 20, which performs time synchronization and wake correlation based on the sensor wake and outputs a set of sensor wakes determined to be the same wake; , A bias grid point search process is performed on a set of sensor tracks determined to be the same track, and a bias grid point search processing unit 40 for calculating a bias estimated value and a bias estimation filter process based on the correlation observation value. A bias estimation filter processing unit 60 that executes and calculates a bias vector estimation value. It is set as the initial value of the vector estimate.
[Selection] Figure 1

Description

  The present invention relates to a sensor bias estimation device that is applied to, for example, a tracking device having a plurality of sensors and estimates the bias (bias error) of each sensor.

  A conventional sensor bias error estimating apparatus observes a plurality of targets based on time-by-time outputs of first and second observers that observe data inputs from first and second sensors, respectively. An observation matrix generator that generates a matrix, an estimate evaluator that calculates a covariance matrix of the bias error estimate from the observation matrix output and the covariance of the observation noise, a temporary estimate from the observation matrix and the estimate evaluator output Estimated value calculator that calculates the bias error value, and is provided between the observation matrix generator and the estimated value evaluator, and only when the minimum eigenvalue of the product of the observed matrix and its transpose matrix is greater than a predetermined constant And a rank determining unit that outputs an observation matrix, and a bias error of the sensor is estimated by a Kalman filter from a cumulative calculation result (see, for example, Patent Document 1).

Next, the purpose of executing bias (bias error) correction will be described with reference to FIG. In this specification, the bias and the bias error are synonymous.
FIG. 14 is an explanatory diagram illustrating how the biases of the two sensors are respectively corrected with a PPI (Plan Position Indicator) display.
In FIG. 14, the sensors A and B constituting the two sensors each have a bias in the azimuth direction, and both the sensor A and the sensor B are simultaneously observing the targets 1, 2, and 3 (FIG. 14). Middle (a) state).

At this time, since the sensors A and B have a bias in the azimuth direction, even if the same target is observed by the sensors A and B, the observation results of the sensors A and B are superimposed. , The targets do not overlap each other and appear to be full (state (b) in the figure). Therefore, in order to improve such a situation, when the biases of the sensors A and B are estimated and the biases are corrected so that the observation results of the sensors A and B are superimposed, the targets are combined into one. They can be overlaid (state (c) in the figure).
In other words, the purpose of correcting the bias is to make the target state quantities made up of the target position, velocity, acceleration, and the like observed by the sensors A and B coincide.

  Next, it is considered that a bias vector composed of components of distance, elevation angle, and azimuth and a state vector composed of components of target position and velocity are estimated by a Kalman filter as shown in Patent Document 1.

Here, the bias vector may be composed of distance and azimuth components, may be composed of elevation and azimuth components, or may be composed of distance and elevation components. It may be composed only of the distance component, may be composed only of the elevation angle component, or may be composed only of the azimuth angle component. That is, the Kalman filter can be configured by arbitrarily setting the order of the bias vector and its components in advance.
Further, the bias vector may be composed of X, Y, and Z components, and the configuration for executing bias estimation is based on various combinations, as in the case of the combinations of the distance, elevation angle, and azimuth components. Can be realized.

  Similarly, the state vector may be configured as a position consisting of a three-dimensional X, Y, Z component and a six-dimensional state vector of velocity, respectively, or a position consisting of a three-dimensional X, Y, Z component, It may be configured as a nine-dimensional state vector of velocity and acceleration, or may be configured as a four-dimensional state vector of position and velocity consisting of two-dimensional X and Y components, respectively. You may comprise as a 6-dimensional state vector of the position which consists of Y component, a speed, and an acceleration. That is, the order of the state vector and its components can be arbitrarily set in advance to form a Kalman filter.

Japanese Patent No. 3302870

However, the prior art has the following problems.
When the bias estimation is executed by the Kalman filter and the least square filter, an initial value of the bias vector estimated value is required. Here, in a conventional sensor bias error estimation apparatus, a method for setting an initial value of a bias vector estimated value is not clarified.
For this reason, when the initial value of the bias vector estimated value is significantly different from the true value of the bias vector, there is a problem that the accuracy of the bias vector estimated value is lowered or the convergence of the bias vector estimated value is delayed. is there.

  The present invention has been made to solve the above-described problems, and an object of the present invention is to obtain a sensor bias estimation apparatus capable of calculating a bias vector estimated value with high accuracy.

  The sensor bias estimation apparatus according to the present invention is an apparatus having a plurality of sensors, and each sensor executes correlation processing based on the observed value and the predicted value to calculate the correlated observed value, and the correlated observed value Is a sensor bias estimation device that estimates the bias of a plurality of sensors and is applied to a device that calculates a sensor wake based on the output and outputs a correlation observation value and a sensor wake, and based on the sensor wake, time synchronization and wake correlation And a synchronization processing means for outputting a set of sensor tracks determined to be the same track and a bias grid point search process for the set of sensor tracks determined to be the same track to calculate a bias estimation value Based on the bias grid point search processing means and the correlation observation value, a bias estimation filter process is executed to calculate a bias vector estimated value. Comprising a filter processing unit, the bias estimation filter processing means is for setting the bias estimate as an initial value of the bias vector estimate.

  According to the sensor bias estimation device of the present invention, the bias estimation filter processing means for executing the bias estimation filter processing based on the correlation observation value from the sensor and calculating the bias vector estimation value is based on the bias grid point search processing means. The bias estimated value calculated by the bias grid point search process is set as the initial value of the bias vector estimated value. Therefore, an initial value of the bias vector estimated value can be appropriately set, and a sensor bias estimating apparatus that can calculate the bias vector estimated value with high accuracy can be obtained.

It is a block block diagram which shows the sensor bias estimation apparatus which concerns on Embodiment 1 of this invention. It is a timing chart for demonstrating the synchronous process by the synchronous process part which concerns on Embodiment 1 of this invention. It is explanatory drawing which shows the wake correlation by the synchronous process part which concerns on Embodiment 1 of this invention. It is explanatory drawing which shows the bias grid point search process in case it has distance, an elevation angle, and an azimuth angle bias by the bias grid point search process part which concerns on Embodiment 1 of this invention. It is explanatory drawing which shows the bias grid point search process in case it has X, Y, Z bias by the bias grid point search process part which concerns on Embodiment 1 of this invention. Based on the bias vector estimated value and its estimated error covariance matrix from the bias estimation filter processing unit according to the first embodiment of the present invention, or the state vector estimated value and its estimated error covariance matrix, the search range of the bias grid point It is explanatory drawing which shows the process which narrows down. It is a flowchart which shows the procedure of the bias grid point search process by the bias grid point search process part which concerns on Embodiment 1 of this invention. It is a block block diagram which shows the sensor bias estimation apparatus which concerns on Embodiment 2 of this invention. It is a block block diagram which shows the sensor bias estimation apparatus which concerns on Embodiment 3 of this invention. It is a block block diagram which shows the sensor bias estimation apparatus which concerns on Embodiment 4 of this invention. It is a block block diagram which shows the sensor bias estimation apparatus which concerns on Embodiment 5 of this invention. It is a block block diagram which shows the sensor bias estimation apparatus which concerns on Embodiment 6 of this invention. It is a block block diagram which shows the sensor bias estimation apparatus which concerns on Embodiment 7 of this invention. It is explanatory drawing which illustrates a mode that each correct | amends the bias of two sensors by PPI display.

  Hereinafter, preferred embodiments of a sensor bias estimation apparatus according to the present invention will be described with reference to the drawings. In the drawings, the same or corresponding parts will be described with the same reference numerals.

Embodiment 1 FIG.
FIG. 1 is a block diagram showing a sensor bias estimation apparatus according to Embodiment 1 of the present invention.
1, the sensor bias estimation apparatus includes a first sensor 10, a second sensor 20, a synchronization processing unit 30, a bias grid point search processing unit 40, an integrated correlation processing unit 50, a bias estimation filter processing unit 60, and a bias database ( Bias DB) 70.

  The first sensor 10 includes a first signal processing unit 11, a first correlation processing unit 12, a first data update processing unit 13, a first prediction processing unit 14, and a first delay processing unit 15. The second sensor 20 includes a second signal processing unit 21, a second correlation processing unit 22, a second data update processing unit 23, a second prediction processing unit 24, and a second delay processing unit 25. The processing from the first signal processing unit 11 to the first delay processing unit 15 in the first sensor 10 and the processing from the second signal processing unit 21 to the second delay processing unit 25 in the second sensor 20 are mutually It is the same processing.

  The first sensor 10 and the second sensor 20 have a bias with respect to absolute coordinates. Further, the bias from both the first sensor 10 and the second sensor 20 can be estimated by the processing from the first sensor 10 to the bias DB 70, or the bias of the second sensor 20 with respect to the first sensor 10 is estimated. You can also.

  Here, the bias of the first sensor 10 and the second sensor 20 with respect to the absolute coordinate is referred to as an absolute bias, and the absolute bias of the second sensor 20 when the absolute bias of the first sensor 10 with respect to the absolute coordinate is 0 is referred to as a relative bias. Call. If the absolute coordinates are the first sensor 10, the bias of the second sensor 20 with respect to the first sensor 10 can be referred to as a relative bias even when the absolute bias of the first sensor 10 is not zero.

  The first sensor 10 and the second sensor 20 may be a three-dimensional sensor such as a radar that observes a distance, an elevation angle, and an azimuth angle, or an IR (Infra Red) sensor that observes an elevation angle and an azimuth angle. Or a two-dimensional sensor such as an ESM (Electric Support Measurements) sensor. The first sensor 10 and the second sensor 20 may be a two-dimensional sensor that observes a distance and an azimuth, may be a one-dimensional sensor that observes only a distance, or only an azimuth. May be a one-dimensional sensor that observes only one angle, or a one-dimensional sensor that observes only the elevation angle.

  Hereinafter, the function of each part of the sensor bias estimation apparatus will be described by taking as an example the case of estimating the relative bias of the second sensor 20 with respect to the first sensor 10. At this time, the 1st sensor 10 and the 2nd sensor 20 are made into the three-dimensional sensor which observes a distance, an elevation angle, and an azimuth as a typical example.

First, the function of the first sensor 10 will be described.
The first signal processing unit 11 obtains observation values of distance, elevation angle, and azimuth at time k from a sensor receiver (not shown) and outputs the observation values to the first correlation processing unit 12.

  The first correlation processing unit 12 includes the observation values of the distance, the elevation angle, and the azimuth at time k input from the first signal processing unit 11 and the distance, elevation angle, and azimuth at the time k input from the first delay processing unit 15. Correlation processing is performed based on the predicted value of the corner. Correlation processing refers to the correspondence between multiple observed values and the target predicted value in the gate taking into account the error of the observed value and the error of the predicted value by the distance between the predicted value and the observed value. This is a process of extracting the closest observed value or a plurality of points close to the predicted value as candidates for the target signal, or extracting the observed value weighted and averaged according to the distance from the predicted value as the target signal candidate.

  In addition, the first correlation processing unit 12 outputs the correlation observation value at time k obtained by executing the correlation processing to the first data update processing unit 13 and the integrated correlation processing unit 50. Here, the correlation observation value is an observation value in the gate.

Based on the Kalman filter data update processing, for example, the first data update processing unit 13 uses, for example, the correlation observation value closest to the predicted value among the correlation observation values input from the first correlation processing unit 12 at time k. The smooth value at time k is calculated. The smooth value is the state vector of the smooth value and its error covariance matrix shown in the above paragraph, and the order of the state vector is set to 3 dimensions, 6 dimensions, or 9 dimensions set in advance. Hereinafter, the smooth value may be referred to as a wake. It is assumed that the error covariance matrix has the same row and column as the order of the state vector.
Further, the first data update processing unit 13 outputs the smooth value at time k to the first prediction processing unit 14 and the synchronization processing unit 30.

The first prediction processing unit 14 extrapolates the state vector of the smooth value and its error covariance matrix at time k input from the first data update processing unit 13 by one sampling based on a motion model assumed in advance. The predicted value at time k + 1 is calculated. The predicted value is the state vector of the predicted value and its error covariance matrix shown in the above paragraph, and the order of the state vector is set to three dimensions, six dimensions, or nine dimensions set in advance.
Further, the first prediction processing unit 14 outputs the predicted value at time k + 1 to the first delay processing unit 15.

  The first delay processing unit 15 delays the prediction value at time k + 1 input from the first prediction processing unit 14 by one sampling, calculates the prediction value at time k, and uses the calculated prediction value for the first correlation processing To the unit 12.

  The functions of the second signal processing unit 21, the second correlation processing unit 22, the second data update processing unit 23, the second prediction processing unit 24, and the second delay processing unit 25 in the second sensor 20 are as described above. Since the functions of the first signal processing unit 11, the first correlation processing unit 12, the first data update processing unit 13, the first prediction processing unit 14, and the first delay processing unit 15 in the first sensor 10 are the same, description thereof is omitted. To do.

  Subsequently, the synchronization processing unit 30 performs time synchronization and wake correlation based on the smooth value input from the first sensor 10 and the smooth value input from the second sensor 20. That is, the synchronization processing unit 30 performs time synchronization and wake correlation based on the wake obtained from the first sensor 10 and the wake obtained from the second sensor 20.

Hereinafter, the time synchronization by the synchronization processing unit 30 will be described with reference to FIG. Here, the wake obtained from the first sensor 10 is referred to as a first sensor wake, and the wake obtained from the second sensor 20 is referred to as a second sensor wake.
FIG. 2 is a timing chart for explaining the synchronization processing by the synchronization processing unit 30 according to the first embodiment of the present invention.

  In FIG. 2, the horizontal axis represents the timeline (A) of the input timing to the synchronization process of the first sensor track, the timeline (B) of the input timing to the synchronization process of the second sensor track, and the process in the synchronization process. A timing timeline (C) is shown. Moreover, the thick bar line orthogonal in the timeline (A) represents the first sensor track at each time, and the thick bar line orthogonal in the timeline (B) represents the second sensor track at each time.

  2 that the first sensor track and the second sensor track have different input timings for the synchronization process. Therefore, in order to compare the first sensor track and the second sensor track at the same time, it is necessary to set the time at a certain reference time. Note that the reference time is determined by the timeline (C).

  At this time, when the time when the wake is generated in the timeline (B) is t (i) and this time t (i) is the reference time (t (k) ≦ t (i) ≦ t (k + 1)), Since the first sensor track is not obtained at time t (i), the synchronization processing unit 30 sets the first sensor track at time t (k) when the first sensor track was obtained before time t (i). Hold until t (i).

That is, the first sensor track at time t (k) is held at the sampling interval Δt (= t (i) −t (k)) from time t (k) to time t (i), and time t ( The first sensor track in i) is calculated.
That is, the synchronization processing unit 30 performs time synchronization at the location (10) in the timeline (C).

Next, the wake correlation by the synchronization processing unit 30 will be described with reference to FIG.
FIG. 3 is an explanatory diagram showing the wake correlation by the synchronization processing unit 30 according to the first embodiment of the present invention.
In FIG. 3, the first sensor wake 101 and the first sensor wake 102 are obtained from the first sensor 10, and the second sensor wake 201, the second sensor wake 202, and the second sensor wake 102 are obtained from the second sensor 20. It is assumed that a 2-sensor wake 203 is obtained. In addition, the gate of the first sensor wake 101 centered on the first sensor wake 101 and the gate of the first sensor wake 102 centered on the first sensor wake 102 are shown in a range surrounded by a dotted line.

  Here, when the second sensor track is in the gate of the first sensor track, the synchronization processing unit 30 determines that the first sensor track and the second sensor track are the same track. In the case of FIG. 3, it is determined that the first sensor track 101 and the second sensor track 201 are the same track, and it is determined that the first sensor track 102 and the second sensor track 203 are the same track.

Such a process for determining the identity between the first sensor track and the second sensor track is referred to as track correlation.
Note that when the wake correlation is executed, the reference coordinates of the first sensor 10 and the second sensor 20 are different from each other, so that after the coordinate conversion to the reference coordinates of the first sensor 10 or the second sensor 20 is performed. Perform wake correlation. In addition, although the gate centered on the first sensor track is set here, the gate is not limited to this, and the gate centered on the second sensor track is set, and the gate is centered on the second sensor track. The same track may be determined based on whether or not one sensor track is entered.

  The synchronization processing unit 30 first performs time synchronization based on the first sensor wake obtained from the first sensor 10 and the second sensor wake obtained from the second sensor 20, and subsequently performs the wake correlation. Execute. At this time, the time synchronization uses the speed of each sensor to hold the track position until the time at which the time synchronization is executed.

  Then, the synchronization processing unit 30 outputs a pair of the first sensor track and the second sensor track determined to be the same track to the bias grid point search processing unit 40. Further, the synchronization processing unit 30 outputs pair information of the first sensor track and the second sensor track to the integrated correlation processing unit 50. The pair information includes not only a pair of the first sensor track and the second sensor track determined to be the same track, but also the first sensor track and the second sensor track that are not determined to be the same track.

Next, the bias grid point search processing unit 40 estimates the relative bias of the second sensor 20 with respect to the first sensor 10 by the bias grid point search process.
Hereinafter, the bias grid point search processing by the bias grid point search processing unit 40 will be described with reference to FIGS.

  FIG. 4 is an explanatory diagram showing a bias grid point search process when the bias grid point search processing unit 40 according to Embodiment 1 of the present invention has distance, elevation angle, and azimuth bias. FIG. 5 is an explanatory diagram showing a bias grid point search process when the bias grid point search processing unit 40 according to Embodiment 1 of the present invention has X, Y, and Z biases. Note that FIG. 4 and FIG. 5 have the same processing contents except for the components, so the bias grid point search processing will be described with reference to FIG.

In FIG. 4, z _k ^Y (under bar) indicates the track of the second sensor 20 at time k, z _k−1 ^Y (under bar) indicates the track of the second sensor 20 at time k−1, and z _{k. -2} ^Y (under bar) indicates the track of the second sensor 20 at time k-2. Z _k ^X (under bar) indicates the track of the first sensor 10 at time k, z _k-1 ^X (under bar) indicates the track of the first sensor 10 at time k−1, and z _k-2. ^X (underbar) indicates the track of the first sensor 10 at time k-2.

Further, Δz _{iR, iE, Az} (underbar) indicate bias candidates of the second sensor 20 with respect to the first sensor 10. There are a plurality of bias candidates, and one of the plurality of bias candidates is selected by the process of FIG. 7 described later.
Here, an image of the bias candidate is as shown on the right side of FIG. 4, and the component of the bias candidate includes a distance component ΔR _iR , an elevation angle component ΔE _iE , and an azimuth angle component ΔAz _iAz , and i _R , i _E , i _Az is an index indicating the position of the bias grid point. Further, the value of this bias candidate changes within a lattice composed of axes of distance, elevation angle, and azimuth angle.

  The search range of the bias grid points is narrowed down based on a bias vector estimated value and its estimated error covariance matrix from a bias estimation filter processing unit 60 described later, or a state vector estimated value and its estimated error covariance matrix.

  FIG. 6 shows a bias vector estimation value and its estimation error covariance matrix from the bias estimation filter processing unit 60 according to the first embodiment of the present invention, or a state vector estimation value and its estimation error covariance matrix. It is explanatory drawing which shows the process which narrows down the search range of a lattice point. In FIG. 6, an image of a two-dimensional plane is shown, but the bias grid point depends on the number of biases to be estimated. For example, when estimating a distance bias, an elevation angle bias, and an azimuth angle bias, the bias grid point is a three-dimensional space.

  At this time, as shown in FIG. 6, the search range of the bias grid point is set to an error ellipsoid obtained from the estimation error covariance matrix of the bias vector estimation value or an error ellipse obtained from the estimation error covariance matrix of the state vector estimation value. The search range of the bias grid points can be narrowed down by setting with a body. Further, by setting the estimated bias value from the bias estimation filter processing unit 60 as the starting point of the lattice point search, the processing for searching from the end of the lattice point becomes unnecessary.

Next, the procedure of bias grid point search processing by the bias grid point search processing unit 40 will be described with reference to the flowchart of FIG.
FIG. 7 is a flowchart showing a procedure of bias grid point search processing by the bias grid point search processing unit 40 according to Embodiment 1 of the present invention. Note that the flowchart of FIG. 8 illustrates processing other than the processing for narrowing down the search range of the bias grid points described above.

First, the bias grid point search processing unit 40, the bias candidate Delta] z _{iR, iE,} before the addition of _Az (underscore), or bias candidate Δz _{iR, iE, Az} (underscore) first sensor track _z ^{k X} unset (Under bar) is set (step S1). This first sensor track is referred to as a bias candidate unset first sensor track.

Subsequently, the bias grid point search processing unit 40 sets a second sensor track z _k ^Y (under bar) determined to be the same track as the first sensor track z _k ^X (under bar) (step S2).

Next, the bias grid point search processing unit 40 _selects one point (see the right side of FIG. 4) in the grid corresponding to the index (i _R , i _E , i _Az ) representing the position of the bias grid point as a bias candidate Δz. Set as _{iR, iE, Az} (underbar) (step S3).

Subsequently, the bias grid point search processing unit 40 adds the bias candidate Δz _{iR, iE, Az} (underbar) set in step S3 to the bias candidate non-set first sensor track z _k ^X (underbar), and the bias Candidate setting first sensor track z _k ^X (underbar) is set (step S4).

Next, the bias grid point search processing unit 40 determines the bias candidate setting first sensor track z _k ^X (underbar) at a certain time (here, time k) from time k−L + 1 to time k. The residual ε ^k _{iR, iE, Az} with the second sensor track z _k ^Y (underbar) is calculated using the following equation (1) (step S5).

The residuals ε ^k _{iR, iE, and Az} may be calculated using the following equation (2) instead of the equation (1).

In the expressions (1) and (2), ‖‖ represents a norm. The norm may be a Euclidean norm or a norm that takes the maximum value of vector components.
The following description will be made using equation (2) as residuals ε ^k _{iR, iE, Az} .

Subsequently, the bias grid point search processing unit 40 determines whether or not a residual has been calculated for L samples from time k−L + 1 to time k (step S6).
If it is determined in step S6 that no residual has been calculated for L samples (ie, No), the bias grid point search processing unit 40 increments the counter i by one (step S7), and step Shifting to S5, the wake residual at the next time is calculated.

On the other hand, if it is determined in step S6 that the residual has been calculated for the L samples (that is, Yes), the bias grid point search processing unit 40 determines the wake residual sum ε _{iR, iE for the} L samples. _{, Az} are calculated using the following equation (3) (step S8).

  Next, the bias grid point search processing unit 40 determines whether or not there are other bias candidates (step S9). Here, when all the bias grid points have already been used, the bias grid point search processing unit 40 determines that there are no other bias candidates, and when there are unused bias grid points remaining. Determines that there is another bias candidate.

If it is determined in step S9 that another bias candidate exists (that is, No), the bias grid point search processing unit 40 increments the counter n by 1 (step S10), and proceeds to step S3. Then, the residual sum ε _{iR, iE, Az} is calculated for another bias candidate.

  On the other hand, if it is determined in step S9 that no other bias candidate exists (that is, Yes), the bias grid point search processing unit 40, based on the residual sum calculated for all the bias candidates, A bias candidate when the difference sum is minimized is determined as a bias estimated value (step S11), and the processing of FIG.

  In other words, in other words, the bias grid point search processing unit 40 moves the bias candidates within the grid as shown on the right side of FIG. ) Is used to calculate the residual sum of wakes for L samples. Then, the bias grid point search processing unit 40 selects a bias candidate when the residual sum of the wakes for L samples is minimized, and sets it as a bias estimated value.

  As described above, the bias grid point search processing unit 40 determines the bias estimated value by the bias grid point search process. In addition, the bias grid point search processing unit 40 outputs the determined bias estimation value to the bias estimation filter processing unit 60.

In addition, the integrated correlation processing unit 50 correlates with the first sensor track in the pair of the first sensor track and the second sensor track determined to be the same track based on the pair information obtained from the synchronization processing unit 30. It is determined whether or not the observed value can be used by the bias estimation filter processing unit 60. Similarly, the integrated correlation processing unit 50 corresponds to the second sensor track in the pair of the first sensor track and the second sensor track determined as the same track based on the pair information obtained from the synchronization processing unit 30. It is determined whether the correlation observation value can be used by the bias estimation filter processing unit 60.
When the integrated correlation processing unit 50 determines that the correlation observation value can be used by the bias estimation filter processing unit 60, the integrated correlation processing unit 50 outputs the correlation observation value to the bias estimation filter processing unit 60 for each determination.

At this time, when the correlation observation value corresponding to the first sensor wake and the correlation observation value corresponding to the second sensor wake are within a predetermined gate range with respect to a predetermined center, the integrated correlation processing unit 50 performs the correlation observation. It is determined that the value can be used by the bias estimation filter processing unit 60.
The predetermined center may be the center of the integrated track of the first sensor track and the second sensor track determined to be the same track by the synchronization processing unit 30 or estimated by the bias estimation filter processing unit 60. It may be the center of the target position. Further, the predetermined gate range may be a gate range of an integrated track of the first sensor track and the second sensor track determined to be the same track by the synchronization processing unit 30, or may be an arbitrarily set gate range. There may be.

  Here, the correlation observation value corresponding to the first sensor 10 obtained from the correlation processing unit 12 and the correlation observation value corresponding to the second sensor 20 obtained from the correlation processing unit 22 are generally the integrated correlation processing unit. 50 is input asynchronously. Note that the correlation observation value corresponding to the first sensor 10 obtained from the correlation processing unit 12 and the correlation observation value corresponding to the second sensor 20 obtained from the correlation processing unit 22 are synchronized with the integrated correlation processing unit 50. Are input in the order of priority of the sensors set in advance, for example, in the order of the first sensor 10 and then the second sensor 20 in accordance with a rule such as processing in order from the smallest sensor number. To do.

  Subsequently, the bias estimation filter processing unit 60 performs state estimation and bias estimation by bias estimation filter processing based on the Kalman filter or the least square method using the correlation observation value from the integrated correlation processing unit 50.

  First, for state estimation, the bias estimation filter processing unit 60 is composed of a state vector composed of each component of the target position and velocity, or each component of the target position, velocity, and acceleration, as shown in the following equation (4). The estimated value of the state vector and its estimated error covariance matrix are calculated. For the order of the target state vector, for example, whether to estimate a 6-dimensional state vector such as Equation (4) or a 9-dimensional state vector such as Equation (5) is determined in advance. Set it.

  In equation (4), (k) indicates time k, x (k) (underscore) indicates a state vector at time k, symbol “′” indicates a matrix and transpose of the vector, and x on the right side of the state vector (K) indicates the position component of the x component, x (k) (dot) indicates the velocity component of the x component, and x (k) (two dots) indicates the acceleration component of the x component. The same applies to the y component and the z component.

  Next, with respect to bias estimation, the bias estimation filter processing unit 60 calculates an estimated value of the bias vector composed of each component of distance, elevation angle, and azimuth and its estimated error covariance matrix as shown in the following equation (6). To do.

In Expression (6), b ^(m) (k) (underbar) indicates a bias vector of the sensor m at time k. Here, m is an identifier representing a sensor, and this identifier m may be set by a number such as m = 1, 2,..., Or m = A, B,. , Z,... May be set by symbols. Here, the identifier m corresponds to either the first sensor 10 or the second sensor 20.

In Equation (6), ΔR ^(m) represents the distance bias of the sensor m, ΔE ^(m) represents the elevation bias of the sensor m, and ΔAz ^(m) represents the azimuth bias of the sensor m.
The type of target bias vector may be a three-dimensional bias vector as shown in Equation (6), or a one-dimensional bias as shown in Equations (7), (8), and (9) below. It may be a vector or a two-dimensional bias vector such as the following equations (10), (11), and (12), and the type of bias vector to be estimated is set in advance for each sensor. Keep it.

  Here, the bias estimation filter processing unit 60 sets the bias estimated value obtained by the bias grid point search processing obtained from the bias grid point search processing unit 40 as the initial value of the bias vector estimated value in the bias estimation. The initial value of the estimated error covariance matrix of the bias vector may be calculated by taking over the previous time value as it is, or a predetermined value is set as the initial value of the estimated error covariance matrix of the bias vector again. May be. Further, the initial value of the bias vector estimated value may be calculated without using the bias estimated value obtained by the grid point search process only once.

  At this time, the state estimation by the bias estimation filter processing unit 60 may be calculated using the state vector estimated value and the estimated error covariance matrix while directly taking over the value of the previous time, or the latest L including the current time. The state vector estimated value and the estimated error covariance matrix may be calculated again at the current time using the correlation observation value of the sample.

Finally, the bias estimation filter processing unit 60 outputs the bias vector estimated value calculated based on the Kalman filter or the least square method to the bias DB 70 as the bias estimation result. In addition, the bias estimation filter processing unit 60 outputs the bias estimation value obtained by the bias grid point search processing obtained from the bias grid point search processing unit 40 to the bias DB 70.
Further, the bias estimation filter processing unit 60 outputs the bias vector estimated value and its estimated error covariance matrix, and the state vector estimated value and its estimated error covariance matrix to the bias grid point search processing unit 40.

Based on the Kalman filter or the least square method, the bias DB 70 accumulates the bias vector estimated value at each time calculated by the bias estimation filter processing unit 60 and the bias estimated value by the bias grid point search process in a database.
Also, the bias DB 70 calculates a moving average value according to the time of the bias vector estimated value and a moving average value according to the time of the bias estimated value, and accumulates these moving average values in the database.

  As described above, according to the first embodiment, the bias estimation filter processing unit performs the bias grid point search processing by the bias grid point search processing unit when performing the bias estimation filter processing by the Kalman filter and the least square filter. The calculated bias estimated value is set as the initial value of the bias vector estimated value. Thereby, the initial value of the bias vector estimated value by the bias estimation filter process can be set appropriately. Therefore, the accuracy of the bias vector estimated value can be improved and the convergence of the bias vector estimated value can be accelerated.

  Although it is possible to calculate the bias estimated value only by the bias grid point search process, when the bias grid point is rough, the present invention calculates the bias estimated value with finer accuracy than the bias grid by the bias grid point search process. Can be calculated. Further, when the bias grid points are made fine, there is a possibility that the bias estimated value can be calculated with high accuracy, but the processing load of the calculation related to the bias grid point search process becomes high.

  At this time, bias the error ellipsoid obtained from the bias vector estimated value output from the bias estimation filter processing means and its estimated error covariance matrix, or the error ellipsoid obtained from the state vector estimated value and its estimated error covariance matrix. By setting the grid point search range, the processing load of the bias grid point search process can be reduced. Further, by setting the search start point of the bias grid point search process as the bias vector estimated value output from the bias estimation filter processing means, the bias grid point search process does not search from the end of the bias grid point to some extent. The estimated bias value can be searched from a position close to the true bias value. As a result, it is possible to realize early convergence of bias estimation accuracy and calculation of a highly accurate bias estimation value.

Embodiment 2. FIG.
FIG. 8 is a block diagram showing a sensor bias estimation apparatus according to Embodiment 2 of the present invention.
In FIG. 8, the sensor bias estimation apparatus includes a bias grid interval control processing unit 80 in addition to the sensor bias estimation apparatus shown in FIG.

The bias grid interval control processing unit 80 sets bias grid points in the bias grid point search processing unit 40 in the following procedure. Further, the bias candidate at the current time, the time, and the grid point search end flag are input from the bias grid point search processing unit 40 to the bias grid interval control processing unit 80.
Here, the lattice point search end flag indicates that a lattice point search is in progress when the lattice point search end flag is 0, for example. When the lattice point search end flag is 1, the search for the lattice point is completed. It is a flag that indicates whether the lattice point search is in progress or the lattice point search is completed, such as indicating that the search is in progress.

First, the bias grid interval control processing unit 80 outputs to the bias grid point search processing unit 40 a control signal for setting a grid interval that roughly searches the bias grid points. Bias grid point search processing unit 40 using equation (3), L samples in track of the residual sum epsilon _{iR, iE,} calculates _Az, residual sum of L samples of the track epsilon _{iR, iE , Az} is minimized, and bias candidates Δz _{iR, iE, Az} (underbars) are output as bias estimation values.

Next, the bias grid interval control processing unit 80 outputs to the bias grid point search processing unit 40 a control signal for finely setting the grid interval with the bias candidate as the center. The bias grid point search processing unit 40 sets the bias grid interval finely, and again calculates the residual sum ε _{iR, iE, Az} of the wakes for L samples using the above equation (3).

  Subsequently, the bias grid interval control processing unit 80 repeatedly outputs a control signal for setting a grid interval for coarsely searching the bias grid points and a control signal for setting the grid interval finely. In addition, the bias grid interval control processing unit 80 is configured such that the difference between the previously calculated L sample wake residual sum and the currently calculated L sample wake residual sum is equal to or less than a predetermined threshold value. In some cases, a control signal for terminating the process is output to the bias grid point search processing unit 40.

  The bias grid interval control processing unit 80 determines that the difference between the search start time obtained from the bias grid point search processing unit 40 and the current time is greater than a predetermined search end determination time threshold value, and the grid. When the point search end flag is 0, it is determined that the search process does not end within a predetermined time. At this time, the bias grid interval control processing unit 80 outputs to the bias grid point search processing unit 40 a control signal that outputs a bias candidate that minimizes the residual sum of the wakes of L samples at the current time. The bias grid point search processing unit 40 outputs to the bias estimation filter processing unit 60 bias candidates that minimize the residual sum of the wakes for L samples at the current time (current time).

  As described above, according to the second embodiment, it is possible to calculate the best bias estimated value at the current time even when the bias grid point search process does not end within a predetermined time.

Embodiment 3 FIG.
FIG. 9 is a block configuration diagram showing a sensor bias estimation apparatus according to Embodiment 3 of the present invention.
In FIG. 9, the sensor bias estimation apparatus includes a bias grid interval resetting processing unit 90 in addition to the sensor bias estimation apparatus shown in FIG.

The bias grid point search processing unit 40 outputs the bias candidate at the current time, the time, and the grid point search end flag to the bias grid interval reset processing unit 90.
The bias grid interval reset processing unit 90 is configured so that the difference between the search start time obtained from the bias grid point search processing unit 40 and the current time is greater than a predetermined grid interval reset time threshold value, and the grid When the point search end flag is 0, a control signal for setting the bias grid interval at the next time to be coarser than the bias grid interval at the current time is output to the bias grid point search processing unit 40.

As described above, according to the third embodiment, the bias grid interval is set to be coarser than the current time even when the bias grid point search process is not expected to end within a predetermined time at the current time. Thus, the bias grid point search process after the next time can be completed within a predetermined time. That is, the best bias estimation value can be calculated within a predetermined time.
The sensor bias estimation apparatus according to Embodiment 3 of the present invention can be combined with Embodiment 2.

Embodiment 4 FIG.
FIG. 10 is a block configuration diagram showing a sensor bias estimation apparatus according to Embodiment 4 of the present invention.
10, the sensor bias estimation apparatus includes a first bias grid point search range resetting processing unit 100 in addition to the sensor bias estimation apparatus shown in FIG.

The bias grid point search processing unit 40 outputs the bias candidate at the current time, the time, and the grid point search end flag to the first bias grid point search range reset processing unit 100.
In the first bias grid point search range reset processing unit 100, the difference between the search start time obtained from the bias grid point search processing unit 40 and the current time is greater than a predetermined search range reset time threshold value. If it is larger and the lattice point search end flag is 0, it is determined that the search process is not completed within a predetermined time.

At this time, the first bias grid point search range resetting processing unit 100 outputs a control signal for narrowing the search range of the bias grid points to the bias grid point search processing unit 40. The bias grid point search processing unit 40 sets a fixed bias grid point search range narrower than the current time.
Note that the first bias grid point search range resetting processing unit 100 uses the error ellipsoid obtained from the bias vector estimation value estimation error covariance matrix shown in FIG. 6 or the state vector estimation value estimation error covariance matrix. A variable bias grid point search range narrower than the current time may be set by multiplying the error ellipsoid obtained from (1) by a coefficient for reducing the radius.

As described above, according to the fourth embodiment, even if the bias grid point search process is not expected to end within a predetermined time at the current time, the search range of the bias grid points after the next time is set. By setting it narrowly, the bias grid point search process after the next time can be completed within a predetermined time. That is, the best bias estimation value can be calculated within a predetermined time.
The sensor bias estimation apparatus according to Embodiment 4 of the present invention can be combined with Embodiment 2 or 3.

Embodiment 5 FIG.
FIG. 11 is a block configuration diagram showing a sensor bias estimation apparatus according to Embodiment 5 of the present invention.
In FIG. 11, this sensor bias estimation apparatus includes a second bias grid point search range resetting processing unit 110 in addition to the sensor bias estimation apparatus shown in FIG. 1.

The bias grid point search processing unit 40 outputs the bias candidate at the current time, the time, and the grid point search end flag to the second bias grid point search range reset processing unit 110.
The second bias grid point search range reset processing unit 110 is configured such that the difference between the search start time obtained from the bias grid point search processing unit 40 and the current time is greater than a predetermined search range reset time threshold value. If it is larger and the grid point search end flag is 1, it is determined that the search process has been completed within a predetermined time.

At this time, the second bias grid point search range resetting processing unit 110 outputs a control signal for widening the search range of the bias grid points to the bias grid point search processing unit 40. The bias grid point search processing unit 40 sets a fixed bias grid point search range wider than the current time.
Note that the second bias grid point search range resetting processing unit 110 performs an error ellipsoid obtained from the estimated error covariance matrix of the bias vector estimated value shown in FIG. 6 or an estimated error covariance matrix of the state vector estimated value. A variable bias grid point search range wider than the current time may be set by multiplying the error ellipsoid obtained from (1) by a coefficient for increasing the radius.

As described above, according to the fifth embodiment, at the current time, the bias grid point search process is expected to end within a predetermined time, and within the bias grid point search range at the current time, When there is no actual true bias value, an actual true bias value existing outside the bias grid point search range can be found by setting a wide bias grid point search range after the next time. That is, the bias estimation accuracy can be improved.
The sensor bias estimation apparatus according to the fifth embodiment of the present invention can be combined with the second or third embodiment.

Embodiment 6 FIG.
FIG. 12 is a block configuration diagram showing a sensor bias estimation apparatus according to Embodiment 6 of the present invention.
12, this sensor bias estimation apparatus includes a bias component estimation order setting processing unit 120 in addition to the sensor bias estimation apparatus shown in FIG.

The bias component estimation order setting processing unit 120 calculates a bias estimation value for a certain component to the bias grid point search processing unit 40 and the bias estimation filter processing unit 60, and then calculates a bias estimation value for the remaining components. Output a control signal.
Hereinafter, after the bias component estimation order setting processing unit 120 calculates a bias estimated value for each component of elevation angle and azimuth with respect to the bias grid point search processing unit 40 and the bias estimation filter processing unit 60, A case where a control signal is output so as to calculate an estimated value will be described as an example.

  At this time, the bias grid point search processing unit 40 sets the bias grid point search process as a range consisting of an elevation angle and an azimuth angle, calculates an elevation angle and azimuth bias estimated value, and calculates the calculation result as a bias estimation filter processing unit. 60.

  Subsequently, the bias estimation filter processing unit 60 sets the elevation angle and azimuth bias estimation values obtained from the bias grid point search processing unit 40 as initial values of bias vector estimation values in bias estimation. Also, the bias estimation filter processing unit 60, based on this initial value, the bias vector estimated value and its estimated error covariance matrix composed of the components of the elevation angle and the azimuth shown in the above equation (10), and the state vector estimation Calculate the value and its estimated error covariance matrix.

Next, the bias grid point search processing unit 40 executes distance bias grid point search processing separately from elevation and azimuth bias grid point search processing.
In the distance bias grid point search processing, the bias grid point search processing unit 40 uses the bias vector estimation value formed from the elevation angle and azimuth components obtained from the bias estimation filter processing unit 60 as a bias candidate in the bias grid point search processing. The elevation angle component and the azimuth angle component of Δz _{iR, iE, Az} (underbar) are set.

Then, the bias grid point search processing unit 40 executes the bias grid point search process shown in FIG. 7 for only the distance component, calculates the distance bias estimated value, and outputs the calculation result to the bias estimation filter processing unit 60. .
In addition, the bias grid point search processing unit 40 outputs the calculated distance bias estimated value to the bias DB 70.

Particularly in the case of a long distance, the cross range error caused by the angle error is worse than the distance error, that is, the range error. In this case, the influence of the angle bias becomes dominant at a long distance compared to the distance bias, and the distance bias cannot be estimated by the bias estimation filter processing.
Therefore, according to the sixth embodiment, the bias estimation accuracy can be improved by, for example, calculating the bias estimation value separately for the elevation angle, the azimuth angle, and the distance.

In the sixth embodiment, as the bias component estimation order, first, the bias estimated value is calculated for each of the elevation angle and azimuth components, and then the bias estimated value is calculated for the distance component. In this example, bias estimation is executed separately for elevation angle, azimuth angle, and distance.
However, the present invention is not limited to this, and bias estimation may be executed separately for distance, elevation angle, and azimuth, or bias estimation may be executed separately for distance, azimuth and elevation, or distance. The bias estimation may be executed separately for the elevation angle and the azimuth angle.
The sensor bias estimation apparatus according to Embodiment 6 of the present invention can be combined with Embodiments 2 to 5.

Embodiment 7 FIG.
FIG. 13 is a block configuration diagram showing a sensor bias estimation apparatus according to Embodiment 7 of the present invention.
In FIG. 13, the sensor bias estimation apparatus includes a bias component order setting processing unit 130 in addition to the sensor bias estimation apparatus shown in FIG.

  In the seventh embodiment, the processing of the first sensor 10 is the same as that of the first embodiment described above. That is, the first sensor 10 obtains the observation values of the distance, the elevation angle, and the azimuth angle from the receiver of the sensor, and finally shows the above formula (4) or (5) from the first data update processing unit 13. Output a smoothed state vector having different components and its error covariance matrix. In the seventh embodiment, a state vector up to a velocity term such as the above equation (4) will be described.

  In the seventh embodiment, the second sensor 20 obtains the observation values of the elevation angle and the azimuth from the receiver of the sensor, and finally, from the second data update processing unit 23, the following equation (13) or A smooth value state vector having the components shown in (14) and its error covariance matrix are output.

  In equations (13) and (14), E (k) represents the elevation angle at time k, Az (k) represents the azimuth angle at time k, and E (k) (dot) represents the elevation angle velocity at time k. , Az (k) (dot) represents the azimuth angular velocity at time k, E (k) (two dots) represents the elevation acceleration at time k, and Az (k) (two dots) represents the azimuth angular acceleration at time k. . In the seventh embodiment, the state vector up to the velocity term as in the above equation (13) will be described.

  The bias component order setting processing unit 130 observes the order of the bias component to be estimated with respect to the synchronization processing unit 30, the bias grid point search processing unit 40, the integrated correlation processing unit 50, and the bias estimation filter processing unit 60. A control signal is output so as to match the order of the component.

  At this time, the synchronization processing unit 30 obtains a smooth value state vector having the component shown in the equation (4) obtained from the first sensor 10, that is, a six-dimensional wake relating to the orthogonal coordinates of x, y, and z, Time synchronization and wake correlation are executed based on a smooth state vector having the component shown in the equation (13) obtained from the sensor 20, that is, a four-dimensional wake relating to the elevation angle and azimuth.

  Here, the wake correlation by the synchronization processing unit 30 is executed using the wake position information. At this time, the 6-dimensional wake related to the x, y, z orthogonal coordinates from the first sensor 10 and the 4-dimensional wake related to the elevation angle and azimuth from the second sensor 20 have different dimensions. Match to the smaller reference coordinate.

  In other words, the synchronization processing unit 30 converts the wake relating to the x, y, z orthogonal coordinates from the first sensor 10 into the second sensor reference coordinates centered on the second sensor 20, and further the second sensor reference coordinates. In, coordinate conversion from orthogonal coordinates to polar coordinates is performed. Thereafter, the synchronization processing unit 30 uses the information of each component of the elevation angle and the azimuth angle in the second sensor reference coordinates to calculate a wake correlation between the wake obtained from the first sensor 10 and the wake obtained from the second sensor 20. Execute.

  Subsequently, the bias grid point search processing unit 40 uses a pair of wakes each having the elevation angle and the azimuth angle of the first sensor 10 and the second sensor 20 obtained by the wake correlation by the synchronization processing unit 30, respectively. A bias grid point search range composed of these elevation angle and azimuth angle components is set. Further, the bias grid point search processing unit 40 calculates the elevation angle and azimuth bias estimated values by the bias grid point search processing, and outputs the calculation results to the bias estimation filter processing unit 60.

  Next, the bias estimation filter processing unit 60 sets the elevation angle and azimuth bias estimation values obtained from the bias grid point search processing unit 40 as initial values of bias vector estimation values in bias estimation. Also, the bias estimation filter processing unit 60 calculates a bias vector estimated value composed of each component of elevation angle and azimuth and an estimated error covariance matrix based on the initial value. Based on this initial value, the bias estimation filter processing unit 60 calculates a state vector of a smooth value and its error covariance matrix in orthogonal coordinates based on the first sensor 10 or the second sensor 20 by state estimation processing. .

  The correlation observation value obtained from the integrated correlation processing unit 50 is the observation position in the three-dimensional orthogonal coordinates of x, y, z with respect to the correlation observation value from the first sensor 10, but from the second sensor 20. Is the observation position in polar coordinates of elevation angle and azimuth.

  Therefore, when the bias estimation filter processing unit 60 executes the state estimation process using the correlation observation value from the second sensor 20, the state vector is a three-dimensional orthogonal coordinate of x, y, z. Since the observation values of the second sensor 20 are the elevation angle and the azimuth angle, and the observation vector components are two-dimensional, the elevation angle and the azimuth angle, the observation model representing the relationship between the state vector and the observation vector is nonlinear. Therefore, when the bias estimation filter processing unit 60 executes the state estimation process using the correlation observation value from the second sensor 20, an extended Kalman filter is used.

  As described above, according to the seventh embodiment, it is possible to improve the bias estimation accuracy even when the orders of the observation vectors obtained by the sensors are different.

  In the seventh embodiment, the bias component order setting processing unit 130 calculates a two-dimensional bias vector estimated value corresponding to the two-dimensional observation vector composed of the elevation angle and the azimuth obtained by the second sensor 20. Explained when to do. However, the bias component order setting processing unit 130 is not limited to this, and the bias component order setting processing unit 130 has a one-dimensional bias vector estimated value having a lower order than the two-dimensional observation vector obtained by the second sensor 20, for example, only the elevation angle component or the azimuth angle. The control signal may be output so as to calculate only the component. The dimension of bias vector estimation value to be calculated is set in advance.

Moreover, in the said Embodiment 7, the case where the 2nd sensor 20 observed an elevation angle and an azimuth was demonstrated. However, the present invention is not limited to this. When the second sensor 20 observes only the distance, observes only the elevation angle, observes only the azimuth angle, observes the distance and elevation angle, observes the distance and azimuth angle. It can also be applied to each case.
The sensor bias estimation apparatus according to Embodiment 7 of the present invention can be combined with Embodiments 2 to 6.

  10 first sensor, 20 second sensor, 30 synchronization processing unit (synchronization processing unit), 40 bias grid point search processing unit (bias grid point search processing unit), 50 integrated correlation processing unit (integrated correlation processing unit), 60 bias Estimation filter processing unit (bias estimation filter processing unit), 80 bias grid interval control processing unit (bias grid interval control processing unit), 90 bias grid interval resetting processing unit (bias grid interval resetting processing unit), 100 first Bias grid point search range reset processing unit (first bias grid point search range reset processing unit), 110 Second bias grid point search range reset processing unit (second bias grid point search range reset processing unit) ), 120 Bias component estimation order setting processing unit (bias component estimation order setting processing means), 130 Bias component order setting processing Part (bias component order setting processing means).

Claims (9)

It is an apparatus having a plurality of sensors, and each sensor performs correlation processing based on the observed value and the predicted value to calculate a correlated observed value, and calculates a sensor wake based on the correlated observed value, A sensor bias estimation device that is applied to a device that outputs the correlation observation value and the sensor track, and estimates bias of the plurality of sensors,
Synchronization processing means for performing time synchronization and wake correlation based on the sensor wake, and outputting a set of sensor wakes determined to be the same wake,
Bias grid point search processing means for performing a bias grid point search process on a set of sensor tracks determined to be the same track, and calculating a bias estimated value;
Bias estimation filter processing means for executing bias estimation filter processing based on the correlation observation value and calculating a bias vector estimation value,
The bias estimation filter processing means sets the bias estimation value as an initial value of the bias vector estimation value.   Provided between the plurality of sensors and the bias estimation filter processing means, and when the correlation observation value corresponding to the sensor track is within a predetermined gate range with respect to a predetermined center, the correlation observation value is 2. The sensor bias estimation apparatus according to claim 1, further comprising integrated correlation processing means for outputting to the bias estimation filter processing means. The bias estimation filter processing means outputs the bias vector estimated value and its estimated error covariance matrix, a state vector estimated value and its estimated error covariance matrix to the bias grid point search processing means,
The bias grid point search processing means determines a search range of the bias grid point search process based on the bias vector estimated value and its estimated error covariance matrix, and the state vector estimated value and its estimated error covariance matrix. The sensor bias estimation device according to claim 1, wherein the sensor bias estimation device is set.   The bias grid interval in the bias grid point search process is set coarsely at the start of the search, and a control signal that is finely set as the search progresses is output to the bias grid point search processing means, and the bias grid point search process ends. If the difference between the search start time obtained from the bias grid point search processing means and the current time is greater than a predetermined search end determination time threshold value, the best bias candidate at the present time 4. The sensor bias estimation according to claim 1, further comprising: a bias grid interval control processing unit that outputs a control signal for outputting a signal to the bias grid point search processing unit. 5. apparatus.   The bias grid point search process has not been completed, and the difference between the search start time obtained from the bias grid point search processing means and the current time is greater than a predetermined grid interval reset time threshold value. In this case, it further comprises bias grid interval reset processing means for outputting a control signal for setting a bias grid interval at the next time coarser than the bias grid interval at the current time to the bias grid point search processing means. The sensor bias estimation apparatus according to any one of claims 1 to 4.   The bias grid point search processing has not been completed, and the difference between the search start time obtained from the bias grid point search processing means and the current time is larger than a predetermined search range reset time threshold value. In this case, the apparatus further comprises first bias grid point search range resetting processing means for outputting a control signal for narrowing a search range of the bias grid point search processing to the bias grid point search processing means. The sensor bias estimation apparatus according to any one of claims 3 to 5.   When the bias grid point search process ends and the difference between the search start time obtained from the bias grid point search processing means and the current time is greater than a predetermined search range resetting time threshold, 4. The apparatus according to claim 3, further comprising second bias grid point search range resetting processing means for outputting a control signal for widening the search range of the bias grid point search processing to the bias grid point search processing means. The sensor bias estimation apparatus according to any one of claims 5 to 6.   As the estimation order of the bias estimation value, after calculating the bias estimation value for a certain component, a control signal for calculating the bias estimation value for the remaining component is output to the bias grid point search processing means and the bias estimation filter processing means The sensor bias estimation apparatus according to any one of claims 1 to 7, further comprising bias component estimation order setting processing means.   When the orders of the observation vectors obtained from the plurality of sensors are different from each other, a control signal for adjusting the order of the bias component to be estimated to the order of the observed component is sent to the synchronization processing means and the bias grid point search. 9. The sensor bias estimation apparatus according to claim 1, further comprising a bias component order setting processing unit that outputs to a processing unit and the bias estimation filter processing unit.

Images