JPH06273173A - Device for estimating bias error of sensor - Google Patents

Abstract

PURPOSE:To calculate the bias error of first and second sensors by calculating the observation matrix of an observation device, calculating the covariance matrix of a bias error estimation value, and then using the covariance matrix, a bias error initial value, and the alignment error between the sensors. CONSTITUTION:An alignment error setting equipment 5 sets the alignment error of a second sensor 2 for a first sensor 1 and then compensates the target position from an observation equipment 4. Adders 7 and 8 add the positions of the sensors 1 and 2 to the target positions from the observation equipment 3 and 4 and then calculate the observation position of the target from the sensors 1 and 2. An observation matrix generator 10 obtains an observation matrix by receiving the output of the adders 7 and 8 and that of an initial bias setting equipment 9 and then an estimation value evaluation equipment 11 calculates estimation value covariance matrix from it. The estimation value calculator 12 obtains a tentative bias error from the output of the evaluation equipment 11 and the generator 10. Then, a subtractor 13 subtracts the output of the calculator 12 from the output of the setting equipment 9 to obtain the bias error of the sensor 1, adds the output of the setting equipment 5 to it with an adder 15 to obtain the bias error of the sensor 2.

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 observation apparatus having a plurality of sensors.

[0002]

2. Description of the Related Art FIG. 4 is a diagram showing an embodiment of a conventional bias error calculating device for a sensor. In FIG. 4, 1 is a first sensor, 2 is a second sensor, 3 is a first observer,
4 is a second observer, 6 is a sensor position setter for inputting the positions of the first sensor 1 and the second sensor 2, and 7 is a target position from the first observer 3 and a sensor position setter 6. Of the second observer 6 and the second sensor position from the sensor position setter 6 are added to calculate the target observation position by adding the first sensor position of Second adder for calculating the target observation position, and 20 for the first adder 7
2 is a second subtractor that calculates the difference between the target observation position from the second adder 8 and the target observation position from the second adder 8, and 21 is a bias error between the first sensor 1 and the second sensor 2. .

Next, the operation will be described. In FIG. 4, the target position is observed by the first sensor 1 and the first observer 3 and the second sensor 2 and the second observer 4, and the first adder 7 and the second adder are observed. Send to 8. Sensor position setter 6
Then, the positions of the first sensor 1 and the second sensor 2 are input. The positions of the first sensor 1 and the second sensor 2 from the sensor position setter 6 are set to the target positions from the first observer 3 and the second observer 4, respectively. The adder 8 adds the values to calculate the target observation position. The outputs of the first adder 7 and the second adder 8 are input to the second subtractor 20,
And the difference between the observation positions from the second adder is calculated. First
If the bias error of the sensor is set to zero, the calculation result of the second subtractor 20 becomes the bias error 21 between the first sensor 1 and the second sensor 2, and this is output.

[0004]

Since the conventional bias error calculating device for a sensor is configured as described above, the bias error of the second sensor 2 with respect to the first sensor 1,
That is, since only the alignment error of the second sensor 2 with respect to the first sensor 1 can be obtained, the bias error of each sensor cannot be calculated, and the bias calculated by the influence of the observation noise included in the observed value cannot be calculated. There was a problem that the reliability of the error was low.

The present invention has been made to solve the above problems, and an object thereof is to estimate the bias error of each of the first sensor and the second sensor.

[0006]

A bias error estimating apparatus for a position observing device according to the present invention comprises an observing position from a first observing device and a second observing device and a second observing device for a first sensor.
The initial value of the bias error is calculated by creating an observation matrix from the sensor alignment error and the initial value of the bias error of the first sensor, and further calculating the covariance matrix of the estimated value from the observation matrix and the covariance matrix of the observation noise. And the covariance matrix of the estimated value, the bias error of the first sensor is estimated, and the bias error of the second sensor is calculated using the alignment error of the second sensor with respect to the first sensor.

[0007]

The bias error estimation value of the position observing device according to the present invention is obtained by calculating the observation matrix of the observing device, calculating the covariance matrix of the bias error estimation value, and setting the bias error of the first sensor as the state variable. The bias error is estimated as the solution of the state equation, and the bias error of the second sensor is further calculated to calculate the bias error of the first sensor and the bias error of the second sensor.

[0008]

【Example】

Embodiment 1 FIG. 1 is a block diagram of a bias error calculation device for a sensor showing an embodiment of the present invention. In the figure, 1-4 and 6-8 are the same as the conventional device. 5 is the second for the first sensor 1
Error setting device for setting the alignment error of the sensor 2 of the above, 9 is an initial bias setting device for setting the initial value of the bias error of the first sensor 1, and 10 is the first adder 7
And an observation matrix generator that calculates k target observation matrices from the target observation position from the second adder 8 and the bias error initial value from the initial bias setter 9, and 11 estimates the bias error from the observation matrix. An estimation value evaluator for calculating a covariance matrix of values, and 12 is an estimation for calculating a temporary value of the bias error estimation value of the sensor from the observation matrix and the covariance matrix of the estimation value of the bias error from the estimation value evaluator 11. A value calculator, 13 is a first subtractor that subtracts a temporary value of the sensor bias error estimated value from the estimated value calculator 12 from the initial value of the sensor bias error from the initial bias setter 9, and 14 is a first subtractor. The bias error estimated value of the first sensor 1 is the calculation result of the subtracter 13, and 15 is the third adder that adds the bias error 14 of the first sensor and the alignment error of the second sensor to the first sensor. , 6 is a bias error of the second sensor at the output of the third adder.

Next, the operation will be described. The correct positions of the k-th target when viewing the k targets from the first sensor 1 and the second sensor 2 are set as P1t and P2t, and the observation positions obtained from the respective sensors are set to P1t.
O and P2tO. Further, the bias error of the first sensor 1 is set to d (dR, dB, dE), and the position vector of the second sensor 2 viewed from the first sensor 1 is set to Pp.
The alignment error between the second sensor and the second sensor is d12
(DR12, dE12, dB12).

P1t, P2t, P1tO, and P2tO are each expressed in an orthogonal coordinate system.

[0011]

[Equation 1]

[0012]

[Equation 2]

[0013]

[Equation 3]

[0014]

[Equation 4]

[0015] Also, obviously,

[0016]

[Equation 5]

Is satisfied. Here as the function F

[0018]

[Equation 6]

Is defined as d (0) (dR) as the initial value of the bias error between the first sensor 1 and the second sensor 2.
(0), dB (0), dE (0) are selected near the equilibrium point of the function F, and the function F is (P1tO, P2tO, d
Taylor expansion around (0)) and ignoring terms of second and higher order,

[0020]

[Equation 7]

Is obtained. Gk, Nk, M in "Equation 7"
k is

[0022]

[Equation 8]

[0023]

[Equation 9]

[0024]

[Equation 10]

It is expressed as If there is no error in the position vector Pp of the first sensor 1 and the second sensor 2, the function F becomes equal to zero. Therefore, the right side of "Equation 7" is set to zero.

[0026]

[Equation 11]

To obtain The left side of “Equation 11” is considered to be an observation value determined by the initial value of the bias error and the observation position. It is considered that the first term on the right side of “Equation 11” is a product of the observation matrix G and the bias error to be obtained as a state variable, and the second and third terms are the product of the observation position error and the observation matrix, that is, noise. To be Therefore, “Equation 11” can be considered as a linear equation of state in which the observation system includes noise.

Now, letting Z be an observed value, H be an observation matrix, and x be a state variable to be obtained.

[0029]

[Equation 12]

It is expressed as If the least squares method is used, the square of the difference between the observed value and the state variable x may be used as the evaluation function J, so if the noise covariance of the observation system is R, then J

[0031]

[Equation 13]

It becomes X that minimizes the evaluation function J is
It is x such that the result of differentiating x by expanding the right side of “Equation 13” becomes zero. Therefore, the required state variable x is

[0033]

[Equation 14]

It is expressed as follows. Here, P (+) is the covariance matrix of the estimated value.

Next, the present invention will be described with reference to FIG.
The first sensor 1 and the second sensor 2 are sensors for observing a target position, and are the first observer 3 and the second sensor 2.
The target position is output by the observer 4 of. The alignment error setter 5 is provided for the second sensor 2 with respect to the first sensor 1.
The alignment error set by the above is sent to the second observer 4 to correct the target position from the second observer 4. The sensor position setter 6 sets the positions of the first sensor 1 and the second sensor 2. In the first adder 7 and the second adder 8, the first sensor 1 is located at the target position from the first observer 3 and the second observer 4.
And the positions of the second sensor 2 are added to calculate target observation positions from the first sensor 1 and the second sensor 2. The initial bias setting device 9 sets the bias error initial value of the first sensor 1. Observation matrix generator 10
Then, by receiving the target observation position from the first adder 7 and the second adder 8 and the bias error initial value from the initial bias setting device 9, The observation matrix Gk represented is calculated and obtained. The estimated value evaluator 11 uses the “equation 14” from the observation matrix Gk from the observation matrix generator 10.
The P (+) inside, that is, the covariance matrix of the estimated value is calculated.
The estimated value calculator 12 calculates “Equation 1” from the covariance matrix from the estimated value evaluator 11 and the observation matrix from the observation matrix generator 10.
The second term on the right side of 4 ″ is calculated as a temporary value of the bias error estimated value. Further, the estimated value calculator 1 is used by the first subtractor 13.
The tentative value of the bias error estimated value from 2 is subtracted from the bias error initial value from the initial bias setter 9 to obtain "Equation 1".
Bias error estimated value 1 of the first sensor 1 represented by 4 "
4 is output. In the third adder 15, the first subtractor 13
Bias error estimation value 14 of the first sensor 1 and the alignment error of the second sensor 2 with respect to the first sensor 1 from the alignment error setter 5 are added to obtain a bias error 16 of the second sensor 2. calculate.

When the target position is observed in this way,
Using the observation position and the set bias error initial value,
The bias error of the second sensor 2 is calculated from the set alignment error by assuming a state equation in which the bias error of the sensor 1 is a state variable, and the bias error of the second sensor 2 is calculated from the set alignment error. The bias error of the second sensor can be estimated.

Embodiment 2 FIG. 2 is a block diagram of a bias error calculating device for a sensor showing Embodiment 2 of the present invention. 1 to 16 are the same as in the first embodiment. An estimated value storage unit 17 stores the bias error estimated value 14 of the first sensor from the first subtractor 13.

Next, the operation will be described. First and
The first adder 7, which observes the target position from the second sensor,
The process up to the process of generating the target observation position by the second adder 8 is the same as in the first embodiment. Next, the previously calculated bias error estimated value is x (−), and the previous estimated value covariance matrix is P.
Put (-). If the least squares method is used, the estimated value of the bias error to be obtained is x, and the square of the difference between x and the previously calculated bias error estimated value x (−), and the observed values z and x.
Since the sum of the difference between (−) and the square is the evaluation function J1, J1 is

[0039]

[Equation 15]

It is expressed as The x that minimizes J1 is such that the result obtained by expanding the right side of J1 and differentiating with respect to x becomes zero. Therefore, the estimated value x to be obtained is

[0041]

[Equation 16]

The present invention will be described with reference to FIG. First,
The operation from observing the target position from the second sensor to generating the target observation position by the first adder 7 and the second adder 8 is the same as that in the first embodiment. The observation matrix generator 10 generates an observation matrix from the target observation position from the first adder 7 and the second adder 8 and the bias error initial value from the initial bias setter 9. Estimated value evaluator 1
In 1, P (+) represented by "Equation 16" is calculated. Further, the estimated value calculator 12 uses the covariance matrix P (+) of the estimated values from the estimated value evaluator 11 to calculate the second side of the right side of “Equation 16”.
Calculate the term. In the first subtractor 13, the estimated value calculator 1
The bias error estimated value 14 of the first sensor represented by "Equation 16" is calculated and output from the output of No. 2 and the previous bias error estimated value from the initial bias setter 9, and is also sent to the estimated value storage unit 17. To do. The estimated value storage unit 17 stores the calculated first sensor bias error estimated value 14 and sends the previously calculated sensor bias error estimated value to the initial bias setting unit 9.

When the target position is observed in this way,
Since the bias error is converged and calculated from the observation position and the previously calculated estimated value, the accuracy is high.

Embodiment 3 FIG. 3 is an embodiment of a bias error device for a sensor showing Embodiment 3 of the present invention. In the figure, 1 to 17 are the same as in the second embodiment. Reference numeral 18 denotes an estimated value convergence determiner that compares the calculated bias error estimated value 14 of the first sensor with an estimated value calculated last time to make a convergence determination, and 19 is an estimated value calculation result.

Next, the operation will be described. First and
The processing from observing the target position from the second sensor to calculating the bias error estimated value 14 of the first sensor is the same as that in the second embodiment. The estimated value convergence determiner 18 compares the estimated value of the bias error calculated last time with the estimated value of the bias error calculated this time, and monitors the degree of convergence of the estimated bias error value 14 of the first sensor. If the estimated bias error value converges below a certain value, the estimated value calculation result 1
Output as 9.

In this way, the estimated value of the bias error is converged and calculated from the target observed position and the estimated value calculated last time, and the convergence judgment is performed, so the accuracy of the estimated value calculation result obtained is good.

[0047]

As described above, according to the present invention, the solution of the state equation in which the bias error of the first sensor is used as the state variable is calculated, and the alignment error of the second sensor with respect to the first sensor is used. Since the bias error of the second sensor is calculated by the above, the bias error of each sensor can be obtained.

Further, when the sensor bias error is calculated by the convergence calculation, there is an effect that the sensor bias error can be accurately estimated.

[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 a configuration diagram of a sensor bias error estimation device according to a second embodiment of the present invention.

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

FIG. 4 is a configuration diagram of a conventional bias error calculation device for a sensor.

[Explanation of symbols]

1 1st sensor 2 2nd sensor 3 1st observer 4 2nd observer 5 Alignment error setting device 6 Sensor position setting device 7 1st adder 8 2nd adder 9 Initial bias setting device 10 Observation matrix generator 11 Estimated value evaluator 12 Estimated value calculator 13 First subtractor 14 Bias error estimated value of first sensor 15 Third adder 16 Bias error of second sensor 17 Estimated value storage 18 Estimated value convergence determiner 19 Estimated value calculation result 20 Second subtractor 21 Bias error between the first sensor and the second sensor

Claims (3)

[Claims] 1. A first sensor for observing k target positions, a first observer corresponding to the first sensor, and a second sensor arranged at a distance from the first sensor. And a second observation device corresponding to the second sensor, in the position observation device, an alignment error setting device for inputting an alignment error of the second sensor with respect to the first sensor; A sensor position setter for setting the positions of the first sensor and the second sensor, a target position from the first observer, and a position of the first sensor from the sensor position setter are added to obtain a target position. A first adder that calculates an observation position and a second addition that calculates the target observation position by adding the target position from the second observer and the position of the second sensor from the sensor position setter Error between the detector and the first sensor Initial bias setting device for setting the initial value of, and an observation matrix for calculating an observation matrix from the outputs of the first and second adders and the first bias value of the first sensor from the initial bias setting device A first sensor from a generator, an estimation value evaluator that calculates a covariance matrix of bias error estimation values from the covariance of the observation noise and the observation matrix, and a covariance matrix of the observation matrix and bias error estimation values Of the bias error of the first sensor by subtracting the temporary value of the bias error of the first sensor from the initial value of the bias error of the first sensor. A first subtractor for calculating an estimated value, and a second for the first sensor from the alignment error setter
And a third adder for calculating the bias error of the second sensor by adding the alignment error of the sensor and the estimated bias error value of the first sensor from the first subtractor. Bias error estimation device for the sensor. 2. An estimated value storage unit for storing the output of the first subtractor for calculating the bias error estimated value of the first sensor and sending the content to the initial bias setting unit. Item 2. A bias error estimating device for a sensor according to item 1. 3. An estimated value determiner for determining convergence of an estimated value from an output of a first subtractor for calculating an estimated bias error value of a first sensor, and repeating the calculation until the estimated bias error value converges. The bias error estimating device for a sensor according to claim 2, wherein: