JPH09288160A - Estimating system of bias error of sensor - Google Patents

Abstract

PROBLEM TO BE SOLVED: To prevent deterioration of the accuracy in estimation of a bias error due to the position of observation of a target and to attain accurate estimation of the bias error by providing a calculator for calculating a proper value of virtual values of an error dispersion matrix of an observation matrix, and others. SOLUTION: Position measuring devices 25a, 25b and 25c borne on movable targets 1-k respectively measure a position of a fixed object in order to specify their own positions and first communication devices 26a, 26b and 26c transmit data on positions in relation to the fixed object to a second communication device 27. The communication device 27 outputs these data on the positions in relation to the fixed object to a relative position storage unit 16. The storage unit 16 stores the minimal proper value of virtual values of an error dispersion matrix of an observation matrix calculated by a proper value calculator 10 and the data on the positions in relation to the fixed object. After the targets 1-k are disposed so that a regular system of the error dispersion matrix be maintained so that accumulation of a computer rounding error due to geometric positional relations between the target and sensors 1 and 2 may not occur, computation for estimation of a bias error is executed.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an apparatus for estimating a bias error of each sensor in a target position measuring apparatus having a plurality of sensors.

[0002]

2. Description of the Related Art FIG. 25 shows, for example, Japanese Patent Laid-Open No. 6-94823.
It is a figure which shows the bias error estimation apparatus of the conventional sensor shown by the publication. In the figure, 1 is a first sensor, 2 is a second sensor, 3 is a first observer, 4 is a second observer,
5 is a sensor position setter for inputting the positions of the first sensor 1 and the second sensor 2, and 6 is the sum of the target position from the first observer 3 and the first sensor position from the sensor position setter 5. And a first adder for calculating the target observation position, and 7 for calculating the target observation position by adding the target position from the second observer 4 and the second sensor position from the sensor position setter 5. 2 is a second adder, 8 is an initial bias setting device that determines the initial value of the bias error of the first sensor 1, and 9 is a target observation position and an initial bias from the first adder 6 and the second adder 7. An observation matrix generator that calculates k target observation matrices from the bias error initial values from the setter 8, 11 is an estimated value evaluator that calculates a covariance matrix of bias error estimated values from the observation matrix, and 12 is Both the observation matrix and the estimated value of the bias error from the estimated value evaluator 11 An estimated value calculator that calculates a temporary value of the sensor bias error estimated value at the sampling time t from the matrix and 13 is calculated from the estimated value calculator 12 from the initial value of the sensor bias error from the initial bias setter 8. A subtractor for subtracting a temporary value of the estimated bias error value of the sensor at the sampling time t, and 14 is an estimate for storing the estimated bias error value 14 of the first sensor and the second sensor from the subtracter 13 at the time t It is a value memory.

Next, the principle will be described. The correct position vector of the k-th target when viewing the k targets from the first sensor 1 and the second sensor 2 is expressed in polar coordinates as in the following expression (1). The observation position vector actually obtained from each sensor is expressed in polar coordinates as in the following equation (2). Further, the bias error between the first sensor 1 and the second sensor 2 is represented by the formula (3), the installation position of the first sensor 1 is the origin, and the position vector of the second sensor 2 is represented by the formula (4) below. As shown in Cartesian coordinates.

[0004]

[Equation 1]

When the correct position vector of the target expressed in polar coordinates and the observed position vector actually obtained from each sensor are expressed in Cartesian coordinates, the following equations (5) and (6) are respectively obtained. Further, the formula (7) is clearly established.

[0006]

[Equation 2]

Equation (8) is defined as the function G, and
Equation (9) is selected near the equilibrium point of the function G as an estimated value of the bias error of the first sensor 1 and the second sensor 2 before one sampling, and the function G is set to (P _1ko underbar (t), P Taylor expansion around _2ko underbar (t), θ ₁ ■, θ ₂ ■) and ignoring terms of second and higher order,
Equation (10) is obtained. However, equation (11) is used.

[0008]

(Equation 3)

The function G is the first sensor 1 and the second sensor 2
If there is no error in the observation vectors P _1ko underbar (t) and P _2ko underbar (t) of, the _values become equal to zero. Therefore, equation (12) is obtained by setting the right side of equation (10) to zero.

[0010]

(Equation 4)

The left side of equation (12) is considered to be an observed value determined by the initial value of the bias error and the observation position. Equation (1
The first term and the second term on the right side of 2) are considered to be the product with the observation matrix, with the bias error to be obtained as the state variable.
The term and the fourth term are the product of the observation position error and the observation matrix, that is,
Considered as noise. Therefore, the equation (12) can be considered as a linear equation of state in which the observation system includes noise. That is, the observation value is Z _t underbar, the observation matrix is H _t , the state variable to be obtained is X underbar, and the observation noise vector is ν.
_{By using t} underbar, the following equation (13) is obtained. Here, each term is represented by equations (14) to (18).

[0012]

(Equation 5)

Since the state vector X underbar in equation (13) is a constant vector that does not depend on the sampling time, the bias error can be estimated by using the transition matrix as the unit matrix and the driving noise as the zero vector in the theory of Kalman filter. If the estimation result of the state vector X underbar at the sampling time t is written as equation (19), equations (20) to (22) are obtained. Here, the expressions (23) to (28) are established. here,

[0014]

(Equation 6)

Note that (σ ² _ikR (t), σ ² _ikE (t), σ
² _ikAz (t) is the variance of the observation error.

Next, the operation will be described. In FIG. 25, a first sensor 1 and a second sensor 2 are sensors for observing the target position, and simultaneously sample the target position. The first observer 3 and the second observer 4 output the target position represented by the equation (2). Further, the sensor position setter 5 has a certain reference position (hereinafter referred to as a reference position)
Set the position of the first sensor 1 and the second sensor 2 with respect to. Here, the positional relationship between the first sensor 1 and the second sensor 2 is represented by Expression (7). In the first adder 6 and the second adder 7, the first sensor 1 and the second sensor 2 are located at the target positions from the first observer 3 and the second observer 4. The positions are added to calculate the target observation position with respect to the reference position. Further, the initial bias setter 8 has the first sensor 1 and the second sensor 2 at the start of operation (t = 1).
The bias error initial value is set, and thereafter (t> 1), the bias error estimated value 14 of the sensor calculated previously from the estimated value storage unit 15 is output (that is, the first term on the right side of Expression (20) is output). To).

In the observation matrix generator 9, the target observation positions from the first adder 6 and the second adder 7 and the bias error initial value from the initial bias setting device 8 or the previously calculated sensor. The bias error estimated value 14 of is received and the observation matrix H _t represented by the equation (15) is calculated.
This corresponds to F _k (t) in equation (16). The estimated value evaluator 11 calculates the covariance matrix P _t in equation (22). Furthermore, in the estimated value calculator 12, the estimated value evaluator 11
Using the covariance matrix of the estimated values from, the temporary value of the bias error estimated value at the sampling time t, that is, the second term on the right side of Expression (20) is calculated. Further, the subtractor 13 converts the bias error estimated value from the estimated value calculator 12 into the bias error initial value from the initial bias setter 8 or the sensor bias error estimated value 1 at the sampling time (t-1).
4, that is, by subtracting from the first term on the right side of Expression (20),
The bias error estimated value 14 of the equation (20) is calculated and output, and is also sent to the estimated value storage 15. The estimated value storage unit 15 stores the calculated estimated value of the sensor bias error at the sampling time t and sends it to the initial bias error estimator 8 as the previously calculated estimated sensor bias error value. Thereafter, this series of processing is repeated until the estimated value of the bias error converges. At this time, each time the initial bias error estimator 8 outputs the estimated sensor bias error value (sampling time (t-1)).

[0018]

In the conventional bias error estimation device for a sensor, if the second term in [] of the equation (20) for obtaining P _t is regular, the estimated bias error value of the sensor, that is, the state. Equation (20) of the equation can be calculated. The necessary and sufficient condition for this is that the rank of the observation matrix is 6. In other words, it is only necessary to observe two or more goals. This condition corresponds to the observability of control theory, but the observable argument assumes that the observation matrix is a constant matrix. However, in the conventional sensor bias error estimation device, the rank of the observation matrix is a discontinuous function of the target observation position and the sensor bias error estimation value one sampling before. Therefore, even when the rank of the observation matrix is 5 or less, the rank of the observation matrix becomes 6 due to a slight difference in the target observation position, etc., and although the bias error can be estimated, there is a phenomenon that it deviates from the true value. There were challenges. On the other hand, in addition to this, if the distance between the target and the sensor is larger than the distance between the two targets, or if it is larger than the distance between the two sensors, or if the elevation angle is constant, the relationship between the sensor and the target observation position is regular. If there is, a phenomenon close to 0% occurs in the calculation of the covariance matrix P _t of the equation (22) of the estimated value evaluator 11, and the phenomenon that the bias error estimated value of the sensor deviates from the true value occurs. There was a problem to do. Moreover, even if this is not the case, there is a problem that the accuracy of bias error estimation deteriorates depending on the target observation position.

The present invention has been made to solve the above problems, and first estimates each of the first sensor and the second sensor by using a plurality of temporary movable targets whose position information is known. The purpose is to accurately obtain the bias error without diverging the calculation.

[0020]

A bias error estimating apparatus for a sensor according to the present invention includes a first observer for observing data input from a target by a first sensor, and
Observation matrix generator that obtains the observation matrix at the output for each time from the second observer that observes with the data input from the second sensor located at a position apart from the sensor, and the error variance of this observation matrix Eigenvalue calculator of error distribution matrix for calculating eigenvalue of temporary value of matrix, minimum eigenvalue of temporary value of error distribution matrix of observation matrix, and fixed target from position measuring means mounted on k movable targets Relative position data of the target is stored in time series in correspondence with the relative position data of the target, and a predetermined value among the minimum eigenvalues of the temporary values of the time series stored in the relative position memory of the target. The target object position evaluator that outputs relative target position data corresponding to the eigenvalue that takes a value, and the relative target position data obtained by the target object position evaluator are transmitted to the above k movable targets, and Target position indicator that indicates the position and observation line An estimated value evaluator that calculates the covariance matrix of the bias error estimation value from the observation matrix output of the generator output and the covariance of the observation noise, and a temporary bias error value from the estimated value evaluator output and the observation matrix output. An estimated value calculator for calculating the bias error of the sensor is estimated from the cumulative calculation result.

Further, instead of the eigenvalue calculator of the error variance matrix, the minimum eigenvalue of the matrix formed by the product of the observation matrix and its transposed matrix, which is a function continuous to each element of the observation matrix, is calculated from the output of the observation matrix generator. A rank evaluation value calculator is provided to estimate the bias error of the sensor from the calculation result.

Further, a rank evaluation value calculator for calculating the minimum eigenvalue of the matrix consisting of the product of the observation matrix and its transposed matrix, which is a function continuous to each element of the observation matrix from the output of the observation matrix generator, is provided, and the error variance It was used together with the matrix eigenvalue calculator.

Further, instead of the estimated value evaluator and the estimated value calculator, a bias function is used from the observation matrix of the observation matrix generator output by using a penalty function for suppressing the divergence of the estimated value of the bias error due to the reduction of the rank of the observation matrix. A bias value of the sensor is estimated from the calculation result by providing an estimated value calculator using a penalty function for estimating.

Further, instead of the estimated value evaluator and the estimated value calculator, a bias function is used to suppress the divergence of the estimated value of the bias error from the observation matrix of the observation matrix generator output and the covariance of the observation noise. A penalty function estimator that calculates the covariance matrix of the error estimate, and a penalty function that suppresses the divergence of the bias error estimate from the observation matrix and the output of the penalty function estimator are used to recursively perform the sensor The bias error of the sensor is estimated from the cumulative calculation result by providing a penalty function type recursive estimation value calculator for calculating the bias error of.

Further, in place of the position memory of the target object, the self-position data from the self-position measuring device mounted on the k movable targets is made to correspond to the minimum eigenvalue of the temporary value with the error variance matrix. The sensor is equipped with a position memory for the movable target that is stored in time series, and the bias error of the sensor is estimated from the calculation result using the output of the observation matrix generator.

Further, in place of the position memory of the target object, the self-position data from the self-position measuring device mounted on the k movable targets is made to correspond to the minimum eigenvalue of the temporary value with the error variance matrix. The sensor is equipped with a position memory for the movable target that is stored in time series, and the bias error of the sensor is estimated from the calculation result using the output of the observation matrix generator.

Further, in place of the position memory of the target object, the self-position data from the self-position measuring device mounted on the k movable targets is made to correspond to the minimum eigenvalue of the temporary value with the error variance matrix. The sensor is equipped with a position memory for the movable target that is stored in time series, and the bias error of the sensor is estimated from the calculation result using the output of the observation matrix generator.

Furthermore, the self-position data is the GPS (Globl Posit) of the self-position measuring device mounted on the k movable targets.
ioning system) device.

[0029]

BEST MODE FOR CARRYING OUT THE INVENTION

Embodiment 1. 1 is a block diagram of a bias error estimating apparatus for a sensor according to a first embodiment of the present invention. In the figure, 1-9 and 11-15 are the same as the conventional device. 10 is an eigenvalue calculator of the error variance matrix for calculating the minimum eigenvalue of the temporary value of the error variance matrix of the observation matrix at the sampling t, 25
a, 25b, 25c are position measuring devices mounted on each movable target 1, movable target 2 or movable target K for measuring the relative position with respect to another target such as a stationary tower or a building, and 26a, 26.
b and 26c transmit relative position data with respect to the fixed target measured by the position observation devices 25a, 25b and 25c to the second communication device 27, or receive position data of the fixed target from the second communication device 27. The first communication device 27, which receives the relative position data with respect to the fixed target object sent from the first communication device 26a, 26b, 26c, or the first communication with the relative position data with the fixed target object. A second communication device for transmitting to the device, 16 is the minimum eigenvalue of the temporary value of the error dispersion matrix of the observation matrix calculated by the eigenvalue calculator 10 of the error dispersion matrix, and the fixed target object received by the second communication device 27. And the relative position data of the target object that is stored in time series as a pair, and 17 is the minimum of the temporary value of the error variance matrix at each sampling time stored in the relative position memory 16 of the target object. Yes value among the relative position data of the fixed target, the position estimator of the target to output the relative position data between the fixed target composed to have the minimum eigenvalue value or more, 1
Reference numeral 8 is a target position indicator for transmitting relative position data with respect to the fixed target output from the position evaluator 17 of the target to each of the k movable targets via the second communication device. FIG. 2 is a flowchart showing the flow of processing of the eigenvalue calculator 10 of the error variance matrix.

Next, the principle will be described. First and
The process up to the process of observing the target position from the second sensor and generating the target observation matrix from the data by the observation matrix generator 9 is the same as the conventional device. Now, let us consider the convergence of the bias error estimated value X _t underbar hat. First, in order to be able to calculate the X _t underbar hat, R _t ^ik is a positive symmetric matrix, F _ik (t) is a regular matrix, and P t is a regular matrix.
^{Since -1} (_) is a semi-positive symmetric matrix, if the following expression (29) is regular, expressions (20) to (22) can be uniquely solved.
In addition, when the equation (30) is set as the at _t underbar, the equation (31) is obtained.

[0031]

(Equation 7)

Therefore, the equations (20) to (22) become the following equations (32) and (33).

[0033]

(Equation 8)

This indicates that when an event close to zero division occurs in the calculation of the equation (22), the estimation result X _t underbar hat of the state vector X _t underbar at the sampling time t may diverge. That is, even if A _t is theoretically regular, the first target 1 with respect to the same target, such as the case where the distance between the first sensor 1 and the second sensor 2 is small or the distance between the target and the sensor is large, etc. When the difference between the observed values of sensor 1 and second sensor 2 is very small,
Accumulation of computer rounding error may cause an event close to zero division in the calculation of Expression (22), and the estimated value of the bias error may be far from the true value. Now, the matrix A _t has a small eigenvalue λ
_{When t} is set, P ⁻¹ (_) becomes a positive symmetric matrix, so
The relationship of Expression (34) is obtained.

[0035]

[Equation 9]

Where || a || underbar is the vector a
Represents the Euclidean norm of the underbar. Therefore,
When positive constant greater than a with the minimum eigenvalue lambda _t of the matrix A _t, the equation (20), the estimation result of the state vector x underscore at the sampling time t, i.e., causes divergence estimated value x _t underscores hat bias error Therefore, the minimum eigenvalue λ _t of the matrix A _t can be calculated by the following equation (3
5) The k targets are moved or stopped so as to satisfy 5), and the state vector x is calculated using (20) to (22).
The underbar estimation result is iteratively calculated.

Next, the operation of the apparatus of the first embodiment to which the above-described principle is applied will be described with reference to FIGS. 1 and 2. In FIG. 1, a target position is observed from the first sensor 1 and the second sensor 2, and the observation matrix generator 9 is used.
The process up to the generation of the target observation matrix is the same as the conventional device. The error variance matrix eigenvalue calculator 10 calculates the minimum eigenvalue of the provisional values of the error variance matrix of the observation matrix.
Next, according to FIG. 2, the eigenvalue calculator 1 of the error variance matrix
The processing flow of 0 will be described. In step 100, the observation matrix from the observation matrix generator 9 at the sample time t is fetched. In step 101, the observation matrix acquired in step 100 and the preset first sensor 1
And the variance of the observation error of the second sensor 2, the matrix A _t, which is a temporary value of the error variance matrix of the observation matrix, is calculated using Equation (29).
Is calculated. In step 102, the minimum eigenvalue of the matrix A _t calculated in step 101 is calculated.

In FIG. 1, the position measuring devices 25a, 25b and 25c mounted on each of the k movable targets measure the position of a stationary object (other fixed target) in order to identify the position of the target itself. To do. The first communication device 26a,
In 26b and 26c, position measuring devices 25a, 25b and 25
The relative position data with the fixed target measured in c
To the communication device 27. The second communication device 27 receives the relative position data with respect to the fixed target transmitted by the first communication devices 26a, 26b, 26c, and outputs it to the relative position memory 16 of the target. In the relative position memory 16 of the target, the relative minimum eigenvalue of the temporary value of the error variance matrix of the observation matrix calculated by the eigenvalue calculator 10 of the error variance matrix and the fixed target received by the second communication device 27 The position data is stored at each sample time. Target relative position evaluator 17
Then, the relative position data with respect to the fixed target when the minimum eigenvalue stored in the relative position memory 16 of the target satisfies the equation (35) is retrieved and output. Here, the value of a certain constant a is a value determined by the accuracy of the computer and the accuracy of the sensor. The target position indicator 18 uses the relative position data with the fixed target object retrieved by the target position evaluator 17 as the second communication device 27.
Out via. The k movable targets are the communication devices 2
The first communication device 26a, 26b, 26c receives the relative position data with respect to the fixed target transmitted in step 7, and the operator of each target uses the position measuring devices 25a, 25b, 25c to detect the relative position with the fixed target. While observing
Each movable target is moved or moved to a position or wake equal to the relative position data with the fixed target received at a, 26b, and 26c. After that, after this movement, the original observation matrix generator generates the necessary matrix at a certain time from a plurality of movable targets and the simultaneous position data to the first and second sensors, similar to the conventional device. To the first sensor 1 and the second sensor 2
Calculate the estimated bias error of.

As described above, the k targets are arranged so that the regularity of the error distribution matrix is maintained so that the accumulation of the computer rounding error caused by the geometrical positional relationship between the target and the sensor does not occur. Since the bias error is estimated and calculated, when the target positions of the movable targets 1 to K are fed back and determined, the relative position data from each of the determined positions allows even temporally temporary data. First
Then, an observation matrix for determining the orientation of the second sensor is obtained, that is, an accurate bias error estimation value of the sensor can be obtained by the measurement value evaluator and the estimation value calculator.

Embodiment 2 An apparatus for evaluating the rank of the observation matrix and arranging k movable targets instead of the eigenvalue calculator of the error variance matrix will be described. Second Embodiment FIG. 3 is a block diagram of a sensor bias error estimating apparatus according to a second embodiment of the present invention. In the figure, 1-9 and 11-15 are the same as the conventional device. Reference numeral 19 denotes a rank evaluation value calculator that calculates a judgment formula obtained from analysis of the rank of the observation matrix at the sampling t. Further, 16 to 18, 25 to 27 are the same as those in the first embodiment. FIG. 2 is a flowchart showing the flow of processing of the floor evaluation value calculator 19.

Next, the principle will be described. First and
Observation matrix generator 9 for observing the target position from the second sensor
The process up to the generation of the target observation matrix is the same as the conventional device. Next, the convergence of the x _t underbar hat will be considered from a viewpoint different from that of the first embodiment. It has been shown in the first embodiment that if the equation (29) is regular, the equations (20) to (22) can be uniquely solved. The necessary and sufficient condition for the matrix A _{t to} be regular is that the rank of the observation matrix H _t is 6. This means that for a three-dimensional sensor, at least two target observations are needed. By the way, the condition that the rank of the observation matrix H _t is 6 corresponds to observability in the control theory, but in the discussion of observability, it is assumed that the observation matrix is a constant matrix. The observation matrix of the device is a function of the target observation position. Therefore, even when the rank of the observation matrix H _t is 5 or less, the rank of the observation matrix H _t becomes 6 due to a slight difference in the target observation position and the like.
Although the bias error of the sensor can be estimated, a phenomenon occurs that it is far from the true value.

As described above, since the conventional device is different from the general theory of control, an analysis index in place of the rank is necessary. The rank of the observation matrix H _t is a discontinuous function of k target observation positions and an estimated value of the bias error of the sensor one sampling before. Therefore, the eigenvalues of the matrix are continuous functions of the elements of the matrix, and the rank of the matrix and the observation H _t is the matrix H _t ^T H _t
Since it matches the rank of, and the rank of the square matrix matches the number of non-zero eigenvalues, a function continuous to each element of the observation matrix H _t is used as an analysis index. That is, the matrix H
made from the product of _t ^T and its a transposed matrix H _t matrix H _t ^T H
_When the minimum eigenvalue of _t is α _t and c is a positive constant, the equation (3
Equation (20) to Equation (22) at the time of sampling that satisfies 6)
It can be seen that it is sufficient to iteratively calculate the estimation result of the state vector x underbar using. Therefore, the calculation by the rank evaluation value calculator 19 for obtaining the minimum eigenvalue α _t from the relative position data from the movable targets 1 to K may be repeated in time.

Next, the operation of the apparatus according to the second embodiment to which the above-described principle is applied will be described with reference to FIGS. 3 and 4. In FIG. 3, the target position is observed from the first sensor 1 and the second sensor 2, and the observation matrix generator 9
The process up to the generation of the target observation matrix is the same as the conventional device. Next, according to FIG. 4, the floor evaluation value calculator 1
The processing flow of No. 9 will be described. The rank evaluator calculator calculates an evaluation value for eliminating an event that does not satisfy the condition for the rank of the observation matrix to be able to calculate the bias error estimated value, depending on the geometrical positional relationship between the target and the sensor. Specifically, it is as follows. In step 100, the observation matrix from the observation matrix generator 9 at the sample time t is fetched. In step 103, a matrix H _t ^T H _t , which is a product of matrices, is calculated from the observation matrix captured in step 100. In step 104, the minimum eigenvalue of the matrix H _t ^T H _t calculated in step 103 is calculated. This means that the eigenvalues of a matrix are continuous functions of each element of that matrix, and
The fact that the rank of a square matrix is consistent with the number of eigenvalues not zero, the observation matrix H _t and rank equal and evaluate the rank of the observation matrix H _t using the eigenvalues of H _t ^T H _t is a square matrix are doing.

In FIG. 3, position measuring devices 25a, 25
From the process of measuring the relative position with respect to the fixed target with b and 25c, the second communication device 27 uses the first communication devices 26a and 26a.
The process from receiving the relative position data with respect to the fixed target transmitted in steps b and 26c and outputting the relative position data to the target relative position storage 16 is the same as in the first embodiment. In the relative position memory 16 of the target object, H _t ^T H calculated by the floor evaluation value calculator 19
The minimum eigenvalue of _t and the relative position data with respect to the fixed target object received by the second communication device 27 are stored for each sample time. The target position evaluator 17 uses the target position memory 1
The relative position data with respect to the fixed target when the minimum eigenvalue stored in 6 satisfies Expression (36) is retrieved and output. The target position indicator 18 uses the relative position data with the fixed target object retrieved by the target position evaluator 17 as the second communication device 27.
Out via. The k movable targets are the communication devices 2
The relative position data with respect to the fixed target transmitted at 7 is received by the first communication devices 26a, 26b, 26c, and the operator of each movable target uses the position measuring devices 25a, 25b, 25c to compare the relative position with the fixed target. While observing the position, it is the communication device 2
Each movable target is moved or moved to a position or track that is equal to the relative position data with the fixed target received at 6a, 26b, and 26c. When the positions of the movable targets 1 to K are determined by feedback, the simultaneous measurement by the first and second sensors at a certain time with respect to the determined position,
After that, the estimated value evaluator 11 and the subsequent processes are performed in the same manner as in the conventional device, and the estimated values of the bias errors of the first sensor 1 and the second sensor 2 are calculated.

As described above, even when the rank of the observation matrix is 5 or less, the rank becomes 6 due to a slight difference in the target observation position and the like, and the bias error estimated value of the sensor deviates from the true value. Since the order of the observation matrix is evaluated and the k targets are arranged so as to avoid the above, the bias error estimation calculation is performed, so that the bias error estimated value of the sensor can be obtained with high accuracy.

Embodiment 3 A case where the first and second embodiments are combined will be described. FIG. 5 shows Embodiment 3 of the present invention.
3 is a configuration diagram of a bias error estimation device for the sensor of FIG. In the figure, 1 to 9 and 11 to 15 are conventional devices, and 10, 10 and 1.
6 to 18, 25 to 27 are the same as the elements used in the first embodiment, and 19 is the same as the elements used in the second embodiment.

Next, the operation of the apparatus of this embodiment will be described with reference to FIG. First, and, second from the sensor to the processing of calculating the minimum eigenvalue of the matrix A _t eigenvalue calculator 10 observed error variance matrix the position of the movable targets embodiments 1
Is the same as The rank evaluation value calculator 19 inputs the observation matrix at the same time as the error variance matrix eigenvalue calculator 10 and H _t ^T
Calculate the minimum eigenvalue of H _t . The transmission of the relative position data from the k movable targets to the fixed target object is the same as in the first embodiment. Based on this data, in the relative position memory 16 of the target object, the minimum eigenvalue of the temporary value of the error variance matrix of the observation matrix calculated by the eigenvalue calculator 10 of the error variance matrix and the H calculated by the rank evaluation value calculator 19 The minimum eigenvalue of _t ^T H _t and the relative position data of the fixed target received by the second communication device 27 are paired and stored at each sample time.
The target relative position evaluator 17 uses the target position memory 1
The relative position data with respect to the fixed target when the minimum eigenvalue of the temporary value and the minimum eigenvalue of H _t ^T H _t of the error variance matrix stored in 6 simultaneously satisfy the equations (35) and (36), respectively. Search and output. The subsequent transmission of the relative position data by the target position indicator 18 is the same as in the first embodiment.

As described above, the accumulation of computer rounding error caused by the geometrical positional relationship between the target and the sensor does not occur, and even when the rank of the observation matrix is 5 or less, the observation position of the target is The number of floors is 6 due to slight differences
In order to avoid the phenomenon that the estimated bias error value of the sensor deviates from the true value, the regularity of the error distribution matrix and the rank of the observation matrix are evaluated and k targets are arranged, and then the bias error An estimation calculation is performed to accurately estimate the bias error of the sensor.

Embodiment 4 In addition to the eigenvalue calculator of the error variance matrix, a device for calculating the bias error at once using a penalty function that prevents the error estimation value from being generated due to the reduction of the rank of the observation matrix will be described. 6 is a block diagram of a bias error estimating apparatus for a sensor according to a fourth embodiment of the present invention. In the figure, 1 to 9 are the conventional device, 10, 16 to 18 and 2
5 to 27 are the same as the elements used in the first embodiment. Reference numeral 20 denotes a penalty function type estimated value calculator that calculates the sensor bias error from the observation matrix using a penalty function that suppresses the divergence of the bias error estimated value.

Next, the principle will be described. First and
Observation matrix generator 9 for observing the target position from the second sensor
The process up to the generation of the target observation matrix is the same as the conventional device. In addition, subsequently, the minimum eigenvalue of the temporary value of the error distribution matrix stored in the position memory 16 of the target object is given by the equation (35).
The process is the same as that of the first embodiment up to the process of moving or moving each of the k movable targets to the position or track corresponding to the relative position data with respect to the fixed target when satisfying the condition. Next, an evaluation function that suppresses the divergence of the bias error is obtained even when the accuracy of one or a plurality of placement positions of the k movable targets is poor and an observation matrix with a risk of diverging the bias error estimation result is obtained. Is defined. That is, the following equation is obtained by applying the method of least squares to the linear state equation of the equation (13) in the conventional apparatus, and adding a term for improving the estimation accuracy of the bias error to the square of the difference between the observed value and the state variable x underbar. The penalty function represented by is used.

[0051]

(Equation 10)

The right side of the equation (37) is a term for preventing the x _t underbar hat from becoming too large even when the minimum eigenvalue of the matrix H _t ^T H _t approaches 0,
L is a diagonal matrix and the diagonal elements are small positive constants. X _t underscore hat to the evaluation function J _t minimized, expand the right side of equation (37), a x _t underbar hat as the result obtained by differentiating the x _t underscore hat becomes zero. That is, the following equation (38) is solved to obtain the equation (39) which is the bias error estimation result.

[0053]

[Equation 11]

Next, the operation of the apparatus of the fourth embodiment to which the above-described principle is applied will be described with reference to FIGS. 6 and 7. 6, the target position is observed from the first sensor 1 and the second sensor 2, and the observation matrix generator 9 is used.
The process up to the generation of the target observation matrix is the same as the conventional device. Further, subsequently, the minimum eigenvalue of the temporary value of the error distribution matrix stored in the relative position memory 16 of the target is calculated by the formula (3
The process up to the process of moving or moving the k movable targets to the position or track corresponding to the relative position data with respect to the fixed target when 5) is satisfied is the same as in the first embodiment. Next, with reference to FIG. 7, a flow of processing of the estimated value 20 by the penalty function after each movable target moves to a predetermined position will be described. The estimated value calculator 20 using the penalty function calculates the bias error estimated value represented by the equation (39) from the observation matrix from the observation matrix generator 9. Specifically, it is as follows. In step 100, the observation matrix generator 9
Take in the observation matrix from. In step 105, the initial value of the bias error from the initial bias setter 8 is fetched. In step 106, the observed values of the state equations represented by the equations (14) to (18) are calculated from the initial values of the bias error captured in step 105 and the position vectors of the first sensor 1 and the second sensor 2. calculate. Step 10
In step 7, the bias value is calculated using equation (39) from the observation value of the state equation calculated in step 106, the observation vector Ht, and the preset variance of the observation errors of the first sensor 1 and the second sensor 2. An estimated error value is calculated, and the bias error estimated value 14 is output in step 108.

As described above, even for the observation matrix in which the estimated value of the bias error is likely to diverge, the evaluation function added with the term for suppressing the occurrence thereof is used, so that the sensor with high reliability and high accuracy can be obtained. A bias error estimate can be obtained.

Embodiment 5 A case where the third and fourth embodiments are combined will be described. FIG. 8 shows a fifth embodiment of the present invention.
3 is a configuration diagram of a bias error estimation device for the sensor of FIG. In the figure, 1 to 9 are conventional devices, and 10, 16 to 19 and 25.
27 are the same as those in the third embodiment, and 20 is the same as those in the fourth embodiment.

Next, the operation of the apparatus of this embodiment will be described with reference to FIG. The minimum eigenvalue of the provisional value of the error variance matrix stored in the relative position memory 16 of the target obtained by observing the relative position with respect to the fixed target from the first and second sensors is expressed by the formula (3
The process up to the process of moving or moving the k targets to the positions or wakes corresponding to the relative position data with respect to the fixed target when 5) is satisfied is the same as in the third embodiment. After that, as in the fourth embodiment, the estimated value calculator 20 using the penalty function calculates the bias error estimated value represented by the equation (39) from the observation matrix from the observation matrix generator 9.
In this way, even for the observation matrix where the estimated bias error value may diverge, the evaluation function with a term to suppress the occurrence is used, so the bias error estimation of the sensor with high reliability and accuracy is possible. You can get the value. The second embodiment and the fourth embodiment may be combined.

Embodiment 6 FIG. A device for obtaining an estimated value covariance matrix from the observation matrix and the covariance of the observation noise using the penalty function instead of the estimated value calculator using the penalty function, and further recursively calculating the error will be described. FIG.
Shows a configuration of a bias error estimating apparatus for a sensor according to a fifth embodiment of the present invention. In the figure, 1-9 and 13-15 are the same as the conventional device. 10, 16 to 18, and 25 to 27 are the same as the elements used in the first embodiment, respectively.
Reference numeral 21 is a penalty function-type estimated value evaluator that calculates a covariance matrix of bias error estimated values from an observation matrix using a penalty function that suppresses divergence of bias error estimated values, 22
Is a recursive calculation of the sensor bias error from the covariance matrix of the bias error estimated value from the penalty function type estimated value evaluator 21 and the observation matrix using a penalty function that suppresses the divergence of the bias error estimated value. It is a penalty function type recursive estimation value calculator.

Next, the principle will be described. The process up to the process of observing the target position from the first sensor 1 and the second sensor 2 and generating the target observation matrix by the observation matrix generator 9 is the same as the conventional device. Further, subsequently, the position or track corresponding to the relative position data with respect to the fixed target when the minimum eigenvalue of the temporary value of the error variance matrix stored in the position storage 16 of the target satisfies the equation (35) is k. The process up to the process of moving or exercising each target is the same as that in the first embodiment. Further, the subsequent processing at the time of the first sampling is the same as that of the fourth embodiment. The covariance matrix of the estimated value at this time is given by the following equation (40).

[0060]

(Equation 12)

For the subsequent samples, the recursive calculation method is used. That is, the following equation is obtained by applying the method of least squares to the linear state equation of the equation (13) in the conventional apparatus, and adding a term for improving the estimation accuracy of the bias error to the square of the difference between the observed value and the state variable x underbar. The penalty function represented by (41) is used. Here, equations (42) and (4
3).

[0062]

(Equation 13)

The right side of the equation (40) is a term for preventing the x _t underbar hat from becoming too large even when the minimum eigenvalue of the matrix H _t ^T H _t becomes close to 0.
Is a diagonal matrix whose diagonal elements are small positive constants. The x _t underbar hat that minimizes the evaluation function J _t is given by the equation (3
Expand the right-hand side of 9), is x _t underscore hat, as a result of the differentiated with respect to x _t underscore hat becomes zero. That is, the following equation (44) is solved to obtain equations (45) to (47), which are the bias error estimation results.

[0064]

[Equation 14]

Next, the operation of the apparatus according to the fifth embodiment to which the above-described principle is applied will be described with reference to FIG. The process up to the process of observing the target position from the first sensor 1 and the second sensor 2 and generating the target observation matrix by the observation matrix generator 9 is the same as the conventional device. Further, subsequently, the position or track corresponding to the relative position data with respect to the fixed target when the minimum eigenvalue of the temporary value of the error distribution matrix stored in the target relative position storage unit 16 satisfies Expression (35) is obtained. The process up to the process of moving or moving the k targets is the same as in the first embodiment. In the penalty function type estimated value evaluator 21, at the time of the first sampling, the covariance matrix of the estimated value of the equation (40) is calculated from the observation matrix from the observation matrix generator 9, and the recursive estimated value calculator 19 using the penalty function is calculated. Send to. In the subsequent samples, the equation (46), that is, the covariance matrix of the estimated value is calculated from the observed matrix from the observed matrix generator 9 and sent to the penalty function type recursive estimated value calculator 22.

Penalty function type recursive estimation value calculator 22
Then, from the covariance matrix from the penalty function type estimated value evaluator 21 and the observation matrix from the observation matrix generator 9, the equation (4
The second term on the right side of 5) is calculated. Further, the subtractor 13 converts the bias error estimated value from the penalty function type recursive estimated value calculator 22 into the bias error initial value from the initial bias setter 8 or the previously calculated sensor bias error estimated value 1
4 is subtracted from 4 to calculate and output the bias error estimated value 14 at the sampling time t in the equation (20), and at the same time, it is sent to the estimated value storage unit 15. Estimated value storage 15
Stores the calculated sensor bias error estimated value 14 at the sampling time t and sends it to the initial bias error estimator 8 as the previously calculated sensor bias error estimated value. Thereafter, this series of processing is repeated until the estimated value of the bias error converges. At this time, each time the initial bias error estimator 8 outputs the estimated sensor bias error value (sampling time (t-1)). In this way, even for the observation matrix where the estimated bias error value may diverge, the evaluation function with a term to suppress the occurrence is used, so the bias error estimation of the sensor with high reliability and accuracy is possible. You can get the value. Further, since the bias error is converged and calculated using the estimated value calculated last time, the accuracy is higher.

Embodiment 7 A case where the third and sixth embodiments are combined will be described. In each of the above embodiments, the case where each movable target 1 to K uses a fixed target on the ground as a stationary object has been described. In addition to this, the stationary object may be a signal from a radio navigation facility. Further, the relative position may be determined in detail by using a plurality of the stationary objects themselves. FIG. 10 is a block diagram of a bias error estimating apparatus for a sensor according to a seventh embodiment of the present invention. In the figure, 1-9 and 13-15 are the conventional device and 10, 16-1.
9, 25 to 27 are the same as those of the third embodiment, and 13 to 15,
21 to 22 are the same as the elements used in the sixth embodiment.

Next, the operation of the apparatus of this embodiment is shown in FIG.
It will be described according to. The minimum eigenvalue and H _t ^T H _t of the temporary value of the error variance matrix stored in the relative position memory 16 of the target observed by observing the position of the movable target from the first and second sensors
Embodiments up to the process of moving or moving the k targets to the position or wake corresponding to the relative position data with the fixed target when the minimum eigenvalues of Eq. (35) and Eq. (36) are simultaneously satisfied. Same as 3. After that, the penalty function type estimated value evaluator 21 calculates the covariance matrix of the estimated values of the expression (40) from the observation matrix from the observation matrix generator 9 at the time of the first sampling, and calculates the recursive estimated values by the penalty function. To the container 19. In the subsequent samples, the equation (46), that is, the covariance matrix of the estimated value is calculated from the observed matrix from the observed matrix generator 9 and sent to the penalty function type recursive estimated value calculator 22. The operation after the penalty function type recursive estimated value calculator 22 is the same as the operation in the sixth embodiment. The second embodiment and the sixth embodiment may be combined.

Embodiment 8. 11 is a block diagram of a bias error estimating apparatus for a sensor according to an eighth embodiment of the present invention. In the figure, 1 to 9 and 11 to 15 are conventional devices,
10, 23, 24, 26a, 26b, 26c and 27 are the same as the elements used in the first embodiment. Two
Reference numerals 8a, 28b and 28c are GPS (Global Positioning System) devices for measuring the positions of the respective k targets.

Next, the operation of the apparatus of this embodiment will be described with reference to FIG.
It will be described according to. In FIG. 11, the first sensor 1,
Also, the process up to observing the target position from the second sensor 2 and generating the target observation matrix by the observation matrix generator 9 is the same as the conventional device. The error variance matrix eigenvalue calculator 10 calculates the minimum eigenvalue of the provisional values of the error variance matrix of the observation matrix. The GPS device 28 mounted on each of the k targets measures the distance from the propagation time of the radio wave from the satellite to determine the position of each of the k targets. The absolute position data of each movable target measured by this GPS device 28
It is received by the communication device 27 and output to the target absolute position memory 23. The target absolute position storage unit 23 stores the minimum eigenvalue of the temporary value of the error distribution matrix of the observation matrix calculated by the eigenvalue calculation unit 10 of the error distribution matrix and the absolute position data of each movable target described above at each sample time. To do. The target absolute position evaluator 24 retrieves and outputs target position data when the minimum eigenvalue stored in the target absolute position storage 23 satisfies the equation (35). Here, the value of a certain constant a is a value determined by the accuracy of the computer and the accuracy of the sensor. The target position indicator 18 sends the absolute position data of the movable target retrieved by the target absolute position evaluator 24 via the second communication device 27. The k movable targets receive the transmitted absolute position data of each movable target, and the operator of each target receives the GP.
While calculating the position of the own device by the S devices 28a, 28b, 28c, move or move each target to a position or wake where it becomes equal to the absolute position data of each movable target received by the communication devices 26a, 26b, 26c. . After that,
The estimated value evaluator 11 and the subsequent processes are performed in the same manner as the conventional device to calculate the estimated values of the bias errors of the first sensor 1 and the second sensor 2.

As described above, the k targets are arranged so that the regularity of the error distribution matrix is maintained so that the accumulation of the computer rounding error caused by the geometrical positional relationship between the target and the sensor does not occur. Since the bias error estimation calculation is performed, it is possible to obtain a highly accurate sensor bias error estimated value by the first and second sensors according to each movable target after movement.

Embodiment 9 FIG. An apparatus for evaluating the rank of the observation matrix and arranging k targets instead of the eigenvalue calculator of the error variance matrix will be described. FIG. 12 is a block diagram of a bias error estimating apparatus for a sensor showing a ninth embodiment of the present invention. In the figure, 1-9 and 11-15 are the same as the conventional device. Further, 18, 23, 24, 26 to 28 are the same as those in the eighth embodiment. Reference numeral 19 denotes a rank evaluation value calculator that calculates a judgment formula obtained from analysis of the rank of the observation matrix at the sampling t.

Next, the operation of the apparatus of this embodiment will be described with reference to FIG.
It will be described according to. In FIG. 12, the first sensor 1,
Also, the process up to observing the target position from the second sensor 2 and generating the target observation matrix by the observation matrix generator 9 is the same as the conventional device. In the rank evaluation value calculator 19, H _t ^T H
_Compute the smallest eigenvalue of _t . Since this is the same as that of the second embodiment, detailed description thereof will be omitted. GPS device 28a,
The process from calculating the position of each of the k movable targets in 28b and 28c to storing the absolute position data of each movable target in the target absolute position memory 23 is the same as in the eighth embodiment. The target absolute position storage unit 23 stores the minimum eigenvalue of H _t ^T H _t calculated by the rank evaluation value calculator 19 and the absolute position data of each movable target at each sample time. The target absolute position evaluator 24 retrieves and outputs absolute position data of each movable target when the minimum eigenvalue stored in the target absolute position storage unit 23 satisfies the equation (36). The operation in which the target position indicator 18 sends the absolute position data of each movable target and the operator of each movable target moves or moves to the position or track is the same as in the eighth embodiment. After that, the processes after the estimated value evaluator 11 are performed similarly to the conventional device, and the first sensor 1 and the second sensor 2 are processed.
Calculate the estimated bias error of.

As described above, even when the rank of the observation matrix is 5 or less, the rank becomes 6 due to a slight difference in the target observation position and the like, and the estimated bias error value of the sensor deviates from the true value. Since the order of the observation matrix is evaluated and the k targets are arranged so as to avoid the above, the bias error estimation calculation is performed, so that the bias error estimated value of the sensor can be obtained with high accuracy.

Tenth Embodiment A case where the eighth and ninth embodiments are combined will be described. 13 is a block diagram of a bias error estimating apparatus for a sensor according to a tenth embodiment of the present invention. In the figure, 1 to 9 and 11 to 15 are conventional devices,
10, 18, 23, 24, 26 to 28 are the eighth embodiment.
19 are the same as the elements used in the ninth embodiment.

Next, the operation of the apparatus of this embodiment is shown in FIG.
It will be described according to. First, and, until the process of calculating the minimum eigenvalue of the matrix A _t eigenvalue calculator 10 observed error variance matrix the position of the target from the second sensor is identical to the eighth embodiment. The floor evaluation value calculator 19 operates similarly to the ninth embodiment. In the target absolute position memory 23, the minimum eigenvalue of the temporary value of the error distribution matrix of the observation matrix calculated by the eigenvalue calculator 10 of the error dispersion matrix and the rank evaluation value calculator 1
The minimum eigenvalue of H _t ^T H _t calculated in 9 and the absolute position data of each movable target received by the second communication device 27 are stored for each sample time. In the target absolute position evaluator 24, the minimum eigenvalue of the provisional value of the error distribution matrix stored in the target absolute position storage unit 23 and the minimum minimum eigenvalue of H _t ^T H _t are given by equation (35) and equation (35), respectively. 36) The absolute position data of each movable target when simultaneously satisfying 36) are retrieved and output. The operation of the target position indicator 18 sending the absolute position data of each movable target and correspondingly moving or exercising each target to the sent position or track by the operator of each movable target is the same as in the eighth embodiment. is there. After that, the estimated value evaluator 11 and the subsequent processes are performed in the same manner as in the conventional device, and the estimated values of the bias errors of the first sensor 1 and the second sensor 2 are calculated.

As described above, the accumulation of computer rounding error caused by the geometrical positional relationship between the target and the sensor does not occur, and even when the rank of the observation matrix is 5 or less, the observation position of the target is The number of floors is 6 due to slight differences
In order to avoid the phenomenon that the estimated bias error value of the sensor deviates from the true value, the regularity of the error distribution matrix and the rank of the observation matrix are evaluated and k targets are arranged, and then the bias error Since the estimation calculation is performed, a highly accurate sensor bias error estimated value can be obtained.

Embodiment 11 FIG. A case where the eighth and fourth embodiments are combined will be described. 14 is a block diagram of a bias error estimating apparatus for a sensor according to Embodiment 11 of the present invention. In the figure, 1 to 9 are the conventional device, 10, 18, 23 and 2
4, 26 to 28 are the eighth embodiment, and 20 is the fourth embodiment.
Are the same as the elements used in.

Next, the operation of the apparatus of this embodiment will be described with reference to FIG.
Follow the instructions below. In FIG. 14, the process up to observing the absolute position of the target from the first sensor 1 and the second sensor 2 and generating the target observation matrix by the observation matrix generator 9 is the same as the conventional device. Further, subsequently, there are k positions or tracks corresponding to the absolute position data of each movable target when the minimum eigenvalue of the temporary value of the error variance matrix stored in the target absolute position storage unit 23 satisfies Expression (35). The process up to moving or exercising the target is the same as in the eighth embodiment. Estimated value calculator 20 based on the subsequent penalty function
The process of calculating the estimated value of the bias error by using Expression (39) is the same as that of the fourth embodiment.

As described above, even for the observation matrix in which the estimated value of the bias error is likely to diverge, the evaluation function added with the term for suppressing the occurrence thereof is used, so that the sensor with high reliability and high accuracy can be obtained. A bias error estimate can be obtained.

Embodiment 12 FIG. A case where the tenth and fourth embodiments are combined will be described. 15 is a block diagram of a sensor bias error estimating device according to a twelfth embodiment of the present invention. In the figure, 1 to 9 are conventional devices, and 10, 18, 1
9, 23, 24, 26 to 28 are the same as those in the tenth embodiment, and 20 is the same as those in the fourth embodiment. The operation of the apparatus of this embodiment is the same as that of the first embodiment.
Since it has been described in Embodiment 0 and Embodiment 4, detailed description will be omitted.

Thirteenth Embodiment A case where the eighth and sixth embodiments are combined will be described. FIG. 16 shows the configuration of a sensor bias error estimating device according to a thirteenth embodiment of the present invention.
In the figure, 1-9 and 13-15 are the same as the conventional device.
10, 18, 23, 24, 26 to 28 are the same as the elements used in the eighth embodiment, and 21, 22 are the same as the elements used in the sixth embodiment. The operation of the apparatus having the configuration of the present embodiment has also been described in each of the above-described embodiments, and thus the detailed description thereof will be redundant and will be omitted.

Embodiment 14 FIG. A case where the tenth and sixth embodiments are combined will be described. FIG. 17 is a block diagram of a sensor bias error estimating apparatus according to a fourteenth embodiment of the present invention. In the figure, 1-9 and 13-15 are conventional devices,
10, 18, 19, 23, 24, 26 to 28 are the same as those of the tenth embodiment, and 21 to 22 are the same as those of the sixth embodiment.

The operation of the apparatus according to the present embodiment has been described in the tenth and sixth embodiments, and detailed description thereof will be omitted.

In the above embodiment, the case where each movable target 1 to K obtains its own position data by GPS has been described. As self-position data is obtained as an absolute value,
Alternatively, signals from a plurality of geostationary satellites may be identified and the self position may be obtained from the combination thereof.

Fifteenth Embodiment The bias error estimation device for a sensor according to the eighth embodiment and later uses the self-position data as GPS.
The case where measurement is performed by the device is shown. As an apparatus for obtaining absolute position data as self-position data, it is possible to perform measurement using a mounted gyro that stores initial coordinates as shown below. 18 to 24 are configuration diagrams of a bias error estimation apparatus for various sensors according to the present embodiment. In the figure, new elements are 29a, 29b, 29.
It is a self-position calculating device by a gyro shown by c.
For these, initial coordinate values are set in advance, and subsequent coordinate values can be obtained by a gyro. Therefore, as in the case of obtaining the self-position data by the GPS device in the previous embodiment, the self-position can be known for each movable target, the absolute position can be transmitted via the second communication device 27, and the target can be obtained. It moves to a predetermined position according to an instruction from the position indicator 18. The configurations of FIGS. 18 to 24 are GPS
Since it corresponds to FIGS. 11 to 17 mounted on the apparatus, detailed description of the operation is omitted.

[0087]

As described above, according to the present invention, the k targets are arranged so that the regularity of the error distribution matrix is maintained, and the bias error is estimated and calculated. This has the effect of suppressing the accumulation of computer rounding errors due to geometrical positional relationships and enabling accurate estimation of sensor bias errors.

Further, since k targets are arranged with a continuous function as a criterion in each element of the observation matrix so that the rank of the observation matrix does not decrease, the bias error is estimated and calculated. There is an effect that the rank drop of the rank due to the geometrical positional relationship of the sensor can be suppressed and the sensor bias error can be accurately estimated.

Further, even when the accuracy of one or a plurality of placement positions of the k targets is poor and an observation matrix that may cause the bias error estimation result to diverge is obtained, the penalty for suppressing the divergence of the bias error is obtained. Since the function is introduced, the accumulation of computer rounding error due to the geometrical influence of the target and the sensor and the divergence of the bias error estimation value due to the rank drop of the observation matrix are suppressed, and the accurate sensor bias error estimation value is obtained. There is an effect to be obtained. Further, the configuration in which the bias error estimated value is converged by using the recursive estimated value calculator has an effect of further improving the accuracy.

According to the configuration in which the elements having the respective characteristics are combined, the characteristics of the above-mentioned individual elements are combined, and there is an effect that a more accurate result can be obtained.

[Brief description of drawings]

FIG. 1 is a configuration diagram of a bias error estimating apparatus for a sensor according to a first embodiment of the present invention.

FIG. 2 is an operation flowchart diagram of an eigenvalue calculator of an error distribution matrix according to the first embodiment of the present invention.

FIG. 3 is a configuration diagram of a bias error estimating device for a sensor according to a second embodiment of the present invention.

FIG. 4 is a flowchart of a rank evaluation value calculator in Embodiment 2 of the present invention.

FIG. 5 is a configuration diagram of a bias error estimating apparatus for a sensor according to a third embodiment of the present invention.

FIG. 6 is a configuration diagram of a bias error estimation device for a sensor according to a fourth embodiment of the present invention.

FIG. 7 is an operation flowchart diagram of an estimated value calculator using a penalty function in the fourth embodiment of the present invention.

FIG. 8 is a configuration diagram of a bias error estimating apparatus for a sensor according to a fifth embodiment of the present invention.

FIG. 9 is a configuration diagram of a bias error estimation device for a sensor according to a sixth embodiment of the present invention.

FIG. 10 is a configuration diagram of a bias error estimating device for a sensor according to a seventh embodiment of the present invention.

FIG. 11 is a configuration diagram of a bias error estimating device for a sensor according to an eighth embodiment of the present invention.

FIG. 12 is a configuration diagram of a bias error estimating apparatus for a sensor according to a ninth embodiment of the present invention.

FIG. 13 is a configuration diagram of a bias error estimating device for a sensor according to a tenth embodiment of the present invention.

FIG. 14 is a configuration diagram of a bias error estimating device for a sensor according to an eleventh embodiment of the present invention.

FIG. 15 is a configuration diagram of a bias error estimating device for a sensor according to a twelfth embodiment of the present invention.

FIG. 16 is a configuration diagram of a bias error estimating device for a sensor according to a thirteenth embodiment of the present invention.

FIG. 17 is a configuration diagram of a bias error estimating device for a sensor according to a fourteenth embodiment of the present invention.

FIG. 18 is a configuration diagram of a bias error estimation device for a sensor according to a fifteenth embodiment of the present invention.

FIG. 19 is a configuration diagram of a bias error estimating apparatus for a sensor according to a fifteenth embodiment of the present invention.

FIG. 20 is a configuration diagram of a bias error estimating apparatus for a sensor according to a fifteenth embodiment of the present invention.

FIG. 21 is a configuration diagram of a bias error estimating device for a sensor according to a fifteenth embodiment of the present invention.

FIG. 22 is a configuration diagram of a bias error estimation device for a sensor according to a fifteenth embodiment of the present invention.

FIG. 23 is a configuration diagram of a bias error estimation device for a sensor according to a fifteenth embodiment of the present invention.

FIG. 24 is a configuration diagram of a bias error estimation device for a sensor according to a fifteenth embodiment of the present invention.

FIG. 25 is a block diagram of a conventional bias error estimation device for a sensor.

[Explanation of symbols]

1 1st sensor, 2 2nd sensor, 3 1st observer, 4 2nd observer, 5 sensor position setter, 6 1st adder, 7 2nd adder, 8 initial bias setting Calculator, 9 observation matrix generator, 10 eigenvalue calculator of error variance matrix, 11 estimated value evaluator, 12 estimated value calculator, 13
Subtractor, 14 Sensor bias error estimate, 15
Estimated value memory, 16 Target relative position memory, 17
Target position evaluator, 18 Target position indicator, 19 Rank evaluation value calculator, 20 Penalty function estimated value calculator, 21 Penalty function type estimated value evaluator, 22 Penalty function type recursive estimated value calculator, 23 Absolute position memory for target, 24 Absolute position evaluator for target, 25a, 2
5b, 25c Position measuring device, 26a, 26b, 26c
First communication device, 27 Second communication device, 28a, 2
8b, 28c GPS device, 29a, 29b, 29c
Gyro self-position calculator.

Claims (9)

[Claims] 1. A first observer for observing by data input from a target by a first sensor and a second observer for observing data by a second sensor located at a position distant from the first sensor. , An observation matrix generator that obtains an observation matrix at each time output from the observer of, an error variance matrix eigenvalue calculator that calculates the eigenvalue of a temporary value of the error variance matrix of the above observation matrix, and the above observation matrix The minimum eigenvalues of the temporary values of the error variance matrix of, and
A relative position memory for the target object that stores relative position data from the position measuring means mounted on the k movable targets in time series in association with each other, and a relative position memory device for the target object. The target position evaluator that outputs the relative target position data corresponding to the eigenvalue that takes a predetermined value among the stored minimum eigenvalues of the temporary values, and the target position evaluator Bias error estimation from the target position indicator that transmits the relative target position data to the k movable targets to indicate the position of the movable target, and the observation matrix output of the observation matrix generator output and the covariance of the observation noise. An estimated value evaluator that calculates a covariance matrix of values, and an estimated value calculator that calculates a provisional bias error value from the estimated value evaluator output and the observation matrix output are provided. The bias error estimation The difference between the bias error estimation apparatus. 2. An eigenvalue calculator of an error variance matrix,
A rank evaluation value calculator for calculating a minimum eigenvalue of a matrix consisting of a product of an observation matrix that is a continuous function for each element of the observation matrix and its transposed matrix from the output of the observation matrix generator is provided. 1. The sensor bias error estimation device according to 1. 3. A rank evaluation value calculator for calculating a minimum eigenvalue of a matrix consisting of a product of an observation matrix, which is a function continuous to each element of the observation matrix, and its transposed matrix from the output of the observation matrix generator, and the error variance is provided. The sensor bias error estimating apparatus according to claim 1, wherein the bias error estimating apparatus is used in combination with a matrix eigenvalue calculator. 4. The bias error is replaced with an estimated value evaluator and an estimated value calculator by using a penalty function for suppressing the divergence of the estimated value of the bias error due to the reduction of the rank of the observation matrix from the observation matrix output from the observation matrix generator. 4. The sensor bias error estimation device according to claim 1, further comprising: an estimated value calculator that calculates a penalty function for estimating the sensor bias error from the calculation result. 5. The bias is estimated by using a penalty function that suppresses the divergence of the estimated value of the bias error from the observation matrix of the observation matrix generator output and the covariance of the observation noise, instead of the estimated value evaluator and the estimated value calculator. Recursive using a penalty function-based estimate evaluator that calculates the covariance matrix of the error estimate, and a penalty function that suppresses the divergence of the bias error estimate from the observation matrix and the penalty function-based estimate evaluator output. 5. The sensor bias error estimation device according to claim 1, further comprising a penalty function type recursive estimation value calculator for calculating the sensor bias error, and estimating the sensor bias error from the cumulative calculation result. 6. The self-position data from the self-position measuring devices mounted on the k movable targets is made to correspond to the minimum eigenvalue of a temporary value with the error variance matrix in place of the relative position memory of the target. A bias error of the sensor according to any one of claims 1 to 3, further comprising a position memory for storing a movable target for time-sequentially storing, and estimating a bias error of the sensor from a calculation result using an output of an observation matrix generator. Estimator. 7. The self-position data from the self-position measuring devices mounted on the k movable targets in place of the position memory of the target is made to correspond to the minimum eigenvalue of the temporary value with the error variance matrix. 5. The sensor bias error estimation device according to claim 4, further comprising a movable target position memory that stores the data in a time series, and estimates the sensor bias error from a calculation result using the observation matrix generator output. 8. The self-position data from the self-position measuring device mounted on the k movable targets is made to correspond to the minimum eigenvalue of the temporary value with the error variance matrix instead of the position memory of the target. The sensor bias error estimation device according to claim 5, further comprising a movable target position memory which stores the time-series data, and estimates the sensor bias error from a calculation result using the observation matrix generator output. 9. The sensor bias error estimation device according to claim 6, wherein the self-position measuring device mounted on the k movable targets is a GPS (Globl Positioning System) device.