JP2019082328A - Position estimation device - Google Patents

Abstract

To enable output of a stable position estimation result when an error occurs in an internal model, and detect an abnormality of the position estimation result within the range of a set error even when an error occurs in an external model for a long time, so as to notify a user of the abnormality.SOLUTION: A position estimation device includes: model abnormality determination means for determining that there is an abnormality in an internal model; internal model error calculation means for calculating an error when there is an abnormality in the internal model; and system abnormality notification means for determining a possibility of deviation of a position output by the position estimation device on the basis of the abnormality states of the internal model and the external model and outputting the possibility as an abnormality of the system state. The position estimation device notifies a user of the possibility of deviation of the output position by the system abnormality notification means when the abnormality is determined by the model abnormality determination means or observation abnormality determination means.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a position estimation device.

  The position estimation device is a device that estimates the position of a vehicle using a global navigation satellite system (GNSS), an inertial measurement unit (IMU), a wheel speed meter, and the like. A mathematical expression used when converting position data output from a device that directly calculates a position, such as GNSS, to a position in a desired coordinate system is called an external model. Also, an equation that sequentially estimates the position of the moving body from related motion parameters such as velocity, attitude, acceleration, and angular velocity is called an internal model.

  Conventionally, many methods can be seen to gradually correct the position calculated using the internal model to the position calculated by the external model using a probability filter represented by a Kalman filter or the like. Since the position correction method using this probability filter corrects the position in consideration of the errors estimated from the internal model and the external model, it is advantageous to be able to calculate the position that is most likely to exist in probability. . However, when there is a so-called outlier in which the position output by the external model is significantly different from the actual position, the output result from the filter is affected by the outlier and has a large error. In order to remove such outliers, χ 2 test and outlier detection and removal by Mahalanobis distance are performed. In such an outlier removal method, the existence probability of the position calculated by the external model is calculated from the position calculated by the internal model and the variance value thereof, and the position with low probability is detected as the outlier.

JP, 2014-228495, A

  The conventional outlier removal method has a mechanism to trust and judge the estimation error in the internal model. Therefore, when the error estimated by the internal model is too small or when there is a phenomenon that can not be expressed as a mathematical expression, the error output by the internal model is small, and the position calculated by the external model is There is a problem that many are all removed as outliers. Such an error of the internal model is called a modeling error.

  When such a modeling error exists, there has conventionally been a method disclosed in the literature 1 in which the calculated position of the external model is always taken into the filter in the stochastic filter. However, in this method, if the position to be calculated in the external model contains a large error for a while, the position to be estimated is largely shifted. At that time, if it is possible to notify the user of the possibility that the estimated position is deviated, it is possible for the user to cope with it, but the conventional method can not recognize the possibility of the estimated position being displaced. There was a problem that it was not possible to say properly to inform Therefore, even if an error occurs in the position estimation device, the user can not determine whether it is an error.

  The features of the present invention for solving the above problems are, for example, as follows.

  Internal model computing means for updating the position of the object from motion parameters of the object, External model computing means for calculating the position itself of the object from position measuring means for measuring the position of the object, Error variance values calculated by the internal model A position estimation device having a probability filter calculation means for correcting the output position of the internal model by the probability filter using the result of the observation abnormality judgment means for judging the outliers of the position calculated by the external model; The position estimation device is based on the model abnormality judging means for judging that there is an abnormality in the model, the internal model error calculating means for calculating the error when there is an abnormality in the internal model, and the state of abnormality of the internal model and the external model. A system abnormality notification unit that determines the possibility of breakage of the output position and outputs the abnormality as a system state abnormality; A position estimation apparatus which reports the possibility of breakage of an output position by means of a system abnormality notifying means when the judging means judges that the abnormality is abnormal and the observation abnormality judging means judges that it is abnormal.

  According to the present invention, it is possible to output a stable position estimation result when an error occurs in the internal model, and even when an error occurs for a long time in the external model, the error in which the abnormality of the position estimation result is set It is possible to detect within the range and to notify the user. Problems, configurations, and effects other than those described above will be apparent from the description of the embodiments below.

It is a figure which shows schematic structure of a position estimation apparatus. It is a figure showing the processing flow of a position estimating device. It is a figure showing data composition of a temporary storage area. It is the figure which showed the processing flow of the observation abnormality judgment means. It is the figure which showed the processing flow of the probability filter calculation means. It is the figure which showed the processing flow of the model abnormality judgment means. It is the figure which showed the image of the error series of time forward direction. It is the figure which showed the image of the error series of time reverse direction. It is a figure showing data composition of an error series data table. It is the figure which showed the processing flow of the internal model error calculation means. It is the figure which showed the processing flow of the system abnormality alerting means.

  Hereinafter, processing for notifying an abnormal state of the position estimation device will be described.

  The present invention relates to an apparatus for estimating the position of oneself, and is used for the movement trajectory and travel control of a moving object such as a vehicle. In the present invention, in a position estimation apparatus using a stochastic filter, a sample having no abnormality in an external model is extracted, and an abnormality in the internal model is judged to detect a modeling error, and an internal model derived from the modeling error is detected. The error variance is learned for each vehicle state. By performing outlier determination using this error variance value, it becomes possible to appropriately determine the outliers of the external model.

  Conventionally, in the external model, when the position to be calculated includes a large error for a while, the position to be estimated is largely shifted. At that time, if it is possible to notify the user of the possibility that the estimated position is deviated, it is possible for the user to cope with it, but the conventional method can not recognize the possibility of the estimated position being displaced. There was a problem that it was not possible to say properly to inform Therefore, in a position estimation apparatus using a probability filter, a model error is detected by extracting a sample having no abnormality in the external model and judging an abnormality in the internal model, and error dispersion of the internal model derived from the modeling error Learn for each vehicle state. By performing outlier determination using this error variance value, it becomes possible to appropriately determine the outliers of the external model.

  The features of the present invention include, for example, the following.

  The observation abnormality judging means judges the outliers by adding the error dispersion of the modeling error of the internal model calculated by the internal model error calculating means to the error dispersion used for the outlier judgment by the Mahalanobis distance.

  The model abnormality judging means judges, using the position outputted by the external model as an end point, whether the series of differences from the external model that can be calculated by updating the position by the internal model will be the same series in the forward and reverse directions in time. Position estimation device.

  A position estimation device for calculating a modeling error of an internal model from a difference between a position calculated by an external model and an estimated position of a position estimation device.

  The system abnormality notifying means determines whether the external model is abnormal or the internal model is abnormal. If the external model is abnormal, the system abnormality other than the position estimation device, or if the internal model is abnormal, the system abnormality of the position estimation device A position estimation device that outputs, as a system state, that there is a possibility that is occurring.

  FIG. 1 shows a typical configuration example of the present invention. In the first embodiment, as an example, using the position obtained by the position measuring unit 101 as the external model, the posture measuring unit 102, the velocity measuring unit 103, the acceleration measuring unit 104, and the angular velocity measuring unit 105 calculate the internal model. Consider such a position estimation device 106. The position estimation device 106 in the first embodiment is premised to be mounted on a vehicle traveling on the ground.

  The position estimation unit 106 includes an external model calculation unit 107 that converts the position output in the coordinate system of the position measurement unit 101 into a coordinate system to be output by the position estimation unit 106, an attitude measurement unit 102, and a speed measurement unit 103. And internal model computing means 108 for calculating the update of the position from the motion parameters of the vehicle obtained from each of the acceleration measuring means 104 and the angular velocity measuring means 105, and the position obtained from the internal model and the position obtained from the external model In the internal model, an observation abnormality judgment unit 109 which judges the presence or absence of an outlier at the position outputted by the position measurement unit, a probability filter calculation unit 110 which calculates a point having the highest existence probability using the Kalman filter etc. Model abnormality judgment means 111 which judges the presence or absence of modeling error, and the abnormality of the internal model Position from the error series data table 112 in which data necessary to make a judgment is stored, the internal model error calculation means 113 for learning the modeling error of the internal model, the abnormal state of the external model and the abnormal state of the internal model It is judged whether the estimation result can not be trusted, and if there is a possibility that it can not be trusted, the system abnormality notification means 114 for notifying that effect and the output value from each calculation means in the position estimation device 106 are temporarily used. There is provided a temporary storage area 115 for storing data.

  A process flow in the position estimation device 106 having the above-described configuration is shown in FIG.

First, in step 201, motion parameters such as position, velocity, and posture are acquired from measurement means for outputting data at a predetermined constant cycle. The measuring means and its motion parameters are the position and position measuring flags from the position measuring means 101, the attitude and posture measuring flags from the attitude measuring means 102, the velocity and speed measuring flags from the speed measuring means 103, and the acceleration from the acceleration measuring means 104 described above. And an acceleration measurement flag and an angular velocity measurement flag from the angular velocity measurement means 105. The measurement flag output from each measurement means is a flag indicating whether or not the information is output from the measurement means at the sample times, and if it is "1", the output is present, and if it is "0", the output is absent. It is assumed that each measuring means is synchronized in time. However, when information on each motion parameter and position is acquired from the measuring means, the process proceeds to step 202. In step 202, the external model calculation unit 107 coordinates-converts the position acquired from the position measurement unit 101 to a position output by the position estimation unit 106 by the external model, and records the coordinate in the temporary storage area 115. In addition, the acquisition flag and the error variance value which is output or set in advance by each measuring unit are stored in the temporary storage area 115. The data configuration of the temporary storage area 115 is shown in FIG.
If there is no output from the position measurement means 101, 0 is set to the acquisition flag of the external model output, and the other processing is skipped. Coordinate conversion is obtained from the following (Equation 1).

Here, (Equation 100) is an external model output value after coordinate conversion, (Equation 101) is an output from the position measurement means 101, and l in (Equation 1) is the position measurement means 101 and the position estimation device 106. It is a vector that represents up to the output position. R _{i in} (Expression 1) is a matrix for converting the i-th position output from the position measurement means 101 into a coordinate system output by the position estimation device 106. Further, R is a matrix for converting a vector representing the position measurement means 101 represented by the vehicle coordinates and the position outputted by the position estimation device 106 into a coordinate system outputted by the position estimation device 106. When the attitude of the vehicle can be acquired by the internal model calculation means 108, that value can be used. In addition, if there is a value specified by design or the like, that value may be used.

  For example, the case where GNSS is used as the position measurement means 101 is considered. The position where the GNSS is obtained is the position of the GNSS antenna, and for the GNSS antenna provided on the vehicle and the GNSS antenna provided on the same vehicle but at a different position from the position estimation device 106, So you can convert the position.

  A coordinate system obtained from GNSS is called an absolute coordinate (a coordinate system in which the east is x axis, the north is y axis, and the ground plane vertical direction is z axis), and the coordinate system fixed to the vehicle is a vehicle coordinate system The traveling direction is referred to as x-axis, y-axis parallel to the rear wheel axis in the left direction of the vehicle, y-plane perpendicular to the xy plane). The difference l between the GNSS antenna and the position estimation device is represented in the vehicle coordinate system.

  Now, it is assumed that the posture θ of the vehicle is (Expression 200) for each of the roll angle, the pitch angle, and the yaw angle. Coordinates that the position estimation device outputs up to the position where the GNSS antenna represented by the vehicle coordinate system and the position estimation device should be output, with the GNSS antenna position represented by absolute coordinates obtained from GNSS as (Expression 201) Assuming that the vector represented by the system is (Expression 202), the position of the position estimation device 106 is represented by absolute coordinates, and the following (Expression 2) is obtained.

However, a matrix for converting R _θ in (Equation 2) from the vehicle coordinate system to the absolute coordinate system is expressed as in (Equation 3) below.

  Next, at step 203, the internal model to be updated is calculated using the information such as the posture and velocity, etc., of the position output by the position estimation device. The internal model in the first embodiment calculates the position update from the posture measurement unit 102, the velocity measurement unit 103, the acceleration measurement unit 104, and the angular velocity measurement unit 105. The update of the position can be calculated, for example, as the following (Equation 4).

When the transformation matrix R _θ from the vehicle coordinate system to the absolute coordinate system is determined by the attitude θ of the vehicle obtained from the attitude measurement means 102,

  However, W in (Equation 4) is called a prediction error variance value, and is a covariance matrix obtained from the standard deviation of the error from each measurement means.

  Further, even if there is no acceleration or angular velocity, the above-described position update can be calculated, and in that case, it can be calculated as in the following (Equation 5).

  Further, the internal model calculation means calculates the vehicle state.

  The vehicle state is automatically determined by the output value from each measuring means used for the internal model. From the function having the output value from each measurement means as a variable, the probability (formula 601) of the vehicle state (formula 600) is expressed as follows (formula 6).

  The function (Expression 602) is assumed to be predetermined. Alternatively, machine learning may be used sequentially from input values and output values to perform learning.

  The order of the processing of the above-described step 202 and step 203 may be reversed because reversing does not affect subsequent processing.

  Next, in step 204, the observation abnormality determination unit 109 determines the presence or absence of an abnormality in the output from the external model calculation unit 107. Abnormality of the observation value is implemented by Chi-square test or the like.

  FIG. 4 shows the flow of the chi-square test.

  First, in step 401, the acquisition flag of the external model output of the temporary storage area 115 is checked to determine whether there is an output from the external model. If there is an output from the external model, the process proceeds to step 402, and if not, the processing of the observation abnormality judging unit 109 is ended.

  Next, in step 402, the position, variance value, and vehicle state output by the internal model are acquired from the temporary storage area 115.

  Next, in step 403, the modeling error variance value of the internal model is added to the prediction error variance value calculated by the internal model stored in the temporary storage area 115. Although calculation of the modeling error variance value of the internal model will be described later, it is assumed that a specified value obtained in advance by actual measurement or the like is input as the initial value.

  Next, in step 404, the Mahalanobis distance of the target external model output value is calculated. The Mahalanobis distance d can be obtained by the following (Equation 7).

  However, assuming that the external model output value is (Equation 700), the position at which the internal model obtained in step 302 is output is (Equation 701), and the variance of the error output by the internal model is represented by Σ.

  Next, at step 405, the Mahalanobis distance is compared with the threshold value obtained from the occurrence probability of the external model output value from the chi-squared distribution. Since the square of the Mahalanobis distance d follows a chi-squared distribution, a value for an arbitrary occurrence probability p is obtained from the chi-square distribution to determine a threshold. The threshold may be constantly calculated or may be a fixed value in advance. If the Mahalanobis distance is smaller than the threshold value than the threshold value, the process proceeds to step 406 and the observation value availability flag is set to 1. If the Mahalanobis distance is greater than the threshold value, the process proceeds to step 407, where the observation value availability flag is set to 0.

  Next, in step 408, the value of the acquisition flag set in the temporary storage area 115 is multiplied by the value of the observation value availability flag to update the value, and the processing of the observation abnormality determination unit 109 is ended. .

  When the observation abnormality judging means 109 of step 204 is finished, the process proceeds to step 205, where the probability filter calculating means 110 calculates an estimated position to be output from the position estimation device 106 and an error variance value of the estimated position. A Kalman filter, a particle filter, etc. can be mentioned as a typical method of a probability filter. In the first embodiment, an estimated position and an error variance value are determined by the Kalman filter from the error variances of the respective models calculated by the external model computing means 107 and the internal model computing means 108.

  FIG. 5 shows the process flow of the probability filter calculation means 110.

  In step 501, the position and the variance are acquired from the output values of the external model and the internal model computing means stored in the temporary storage area 115. In the external model, the acquisition flag is acquired, and in the internal model, the vehicle state is acquired as well.

  Next, in step 502, it is determined whether the acquisition flag of the external model is one. If the acquisition flag is 1, the process moves to step 503. If the acquisition flag is 0, the process proceeds to step 504. In step 503, since the external model output value exists and is not an outlier, the error variance value of the external model is set, and the process proceeds to step 505. In step 504, since the external model output value does not exist or is an outlier, a predetermined maximum value is set as the error variance value of the external model. Also, the position output from the external model calculation means 107 is set to the same value as the value output from the internal model calculation means 108.

  Next, at step 505, the innovation v in the Kalman filter is calculated. The innovation v is obtained from the position X output from the external model calculation means 107 and the updated position x output from the internal model calculation means 108 as shown in the following (Equation 8).

  Next, in step 506, the Kalman gain K is calculated. The Karman gain K can be obtained as in the following (Equation 9).

  Next, at step 507, the estimated position is calculated. The estimated position can be calculated as (Equation 10) below.

  Next, at step 508, the estimated error variance is calculated. The estimated error variance can be calculated as (Equation 11) below.

  Finally, in step 509, the calculated estimated position, estimated error variance, and innovation value are recorded in the temporary storage area 115, and the estimated position is output from the position estimation device 106, and the processing of the probability filter calculation means 110 is ended.

  When the process of the probability filter calculation means 110 is completed, the process proceeds to step 207, where the model abnormality judgment means 111 judges the presence or absence of abnormality in the internal model. In this anomaly judgment, the autocorrelation of the difference with the internal model at each sample time of the external model is calculated, and the signal difference from the autocorrelation so far is considered as a time series signal, and independent component analysis etc. If it is determined that there is an abnormality, it is determined that there is an abnormality because it is different from the previous signal.

  A specific processing flow is shown in FIG.

  First, at step 601, the output value from the external model and the output value from the internal model are acquired from the temporary storage area 115. Next, at step 602, the amount of movement is calculated by integrating the amount of movement of each position of the internal model. This calculation is an integration from the sample in which the value from the external model was acquired last time, and is expressed as in (Expression 12) below.

  Here, dx is the amount of movement at the time of sampling, and the left side of (Expression 12) is an integration of t times from the sample for which the value from the external model was previously obtained. The error series data table 112 is updated with the calculated rotation amount and movement amount.

  Next, the process proceeds to step 603, where it is checked whether the acquisition flag of the external model is one. If the output value of the external model is available, that is, if the external model acquisition flag is 1, the process proceeds to step 604. If the output value of the external model is not usable, that is, if the external model acquisition flag is 0, the process proceeds to step 608.

  In step 604, an error sequence is calculated for each of the time forward direction and the reverse direction. The error series is the difference between the value output from the external model and the value according to the movement amount integrated by the internal model. When the starting point of the movement amount integrated by the internal model is closer to the current time in time, it is reverse in time, and when it is farther away, it is defined as forward in time. 7 and 8 show examples of time forward and backward error sequences.

  The error sequence in the time forward direction is determined by the difference 704 between the position obtained by adding the vector 702 obtained by integrating the internal model output value from the output value 701 of the external model as the start point and the output position 703 of the next external model. It is a series of found values. This time forward error sequence can be calculated as (Equation 13) below.

  Here, R is a matrix representing the rotation at the starting point, and when the attitude of the vehicle can be acquired by the attitude measurement means 102 or the internal model calculation means 108, that value is used.

  Similarly, the error sequence in the time reverse direction is a position obtained by subtracting the vector 802 obtained by integrating the internal model output value with respect to time from the output value 801 of the external model as the starting point, and It is a series of values obtained by the difference 804 with the output position 803 of the previous external model. This time-reversed error sequence can be calculated as in (Equation 14) below.

  The error series obtained by this calculation is set in the error series data table. An example of the error series data table is shown in FIG. The error series data is a ring buffer type table capable of storing up to m pieces of data in time order. Although all time samples are shown in FIG. 9 for the sake of explanation, the data actually stored is only when the external model acquisition flag is 1.

  Next, in step 605, the autocovariance matrix is calculated from the m error sequences created in each of the time forward direction and the reverse direction, the eigenvectors are determined, and the angle formed by the eigenvectors in the time forward direction and the time reverse direction is calculated. Do.

  The autocorrelation variance matrix of the error sequence is obtained as in the following (Equation 15).

  Assuming that the eigenvectors of the autocorrelation matrix in the time forward direction and the reverse direction obtained by the above equation are represented as bf and br, the angle formed by the eigenvectors of the two autocorrelation matrices is calculated as in the following (Equation 16) it can.

  Next, in step 606, it is determined whether the time series forward and backward error series data are identical. If there is a modeling error, the error accumulates in the posture and velocity over time, so that there is a difference between the time-sequential and backward error series data. Therefore, it is compared whether the left side of the angle (equation 16) formed by the eigenvectors of the two autocorrelation matrices calculated in step 605 is larger than the threshold. If the left side of the angle (Equation 16) formed by the eigenvectors is larger than the threshold, it can be considered that there is a difference between the error series data in the time forward direction and the reverse direction. Transition. If the left side of the angle (Equation 16) formed by the eigenvectors is smaller than the threshold, it can be considered that there is no difference in the error series data in the time forward direction and the reverse direction, and there is no abnormality in the internal model. Migrate to

  In step 607, a model abnormality flag indicating the presence or absence of an abnormality in the internal model is set to 1, and output, and the process ends.

  At step 608, the model abnormality flag is set to 0, and the process is terminated.

  When the setting of the model abnormality flag is completed in step 206, next, in step 207, the process is changed according to the model abnormality flag. If the model abnormality flag is set to 1, the process proceeds to step 208 to calculate an internal model error. If the model abnormality flag is set to 0, the process proceeds to step 209 to determine whether to notify of a system abnormality.

  At step 208, the internal model error calculation means 113 calculates a modeling error of the internal model. FIG. 10 shows a processing flow for calculating the modeling error of the internal model.

  First, in step 1001, information necessary for calculation is acquired from the temporary storage area 115. Information necessary for calculation includes an external model acquisition flag, an estimated position calculated by the internal model calculation means 108, current vehicle state, speed, acceleration, attitude, angular velocity, innovation calculated in the probability filter calculation means 110, And all vehicle states updated in the internal model error calculation stage.

  Next, at step 1002, the external model acquisition flag is checked. If the external model acquisition flag is 1, the process proceeds to step 1003. If the external model acquisition flag is 0, the modeling error can not be calculated, so the process proceeds to step 1007 and the modeling error calculation impossible flag is set to 1. The modeling error calculation impossible flag is set to 0 as an initial value, and it is assumed that it is initialized every start.

  At step 1003, the average value of modeling errors in the current vehicle state acquired at step 1001 is updated. Since the average value of the modeling error is equal to the average value of the innovation v calculated by the probability filter calculation means 110, the following equation (17) can be obtained from the average value E [v] of the current vehicle state acquired in step 1001: You can calculate as follows.

  Where N is the number of samples stored in each vehicle state.

Next, in step 1004, the variance value E [v ² ] of the modeling error in the current vehicle state acquired in step 1001 is updated. From the value of the innovation calculated by the probability filter calculation means 110, it may be obtained sequentially as in the following (Expression 18).

  Next, in step 1005, the average value and the variance value of the modeling error calculated in step 1003 and step 1004 are overwritten and updated in the temporary storage area 115.

  Finally, in step 1006, the transition probability of the vehicle state (equation 19) is updated. The transition probability (Equation 19) to each vehicle state from the vehicle state (Equation 190) at a certain time to the vehicle state (Equation 191) at the next sample time is calculated as in (Equation 19) below.

  However, the transition probability (equation 19) is expressed by a matrix such as the following (equation 20).

  When the above equation is used as it is, the transition probability of the vehicle state may not be stable in many cases. Therefore, a method of gradually converging using a probability gradient method or the like may be employed.

  At this time, when the transition probability from a certain state to a certain state is higher than the threshold value in both directions, the vehicle state is not stable, and therefore, it merges into one vehicle state. At this time, the modeling error average value and the modeling error dispersion value set for each vehicle state are updated using the respective sample numbers.

  When step 1006 or step 1007 ends, the processing of the internal model error calculation means 113 ends.

  Next, at step 209, a modeling error calculation impossible flag is checked. When the modeling error calculation impossible flag is 0, since the setting of the modeling error is completed, the process is ended. When the modeling error calculation impossible flag is 1, the system abnormality notification unit 114 is activated to determine the abnormality of the system.

  When the system abnormality notification unit 114 is not activated and the processing of the internal model error calculation unit 113 is completed, the processing of the position estimation device 106 is ended to prepare for the processing at the next activation.

  On the other hand, if no model abnormality is found in step 207 in the processing flow of the position estimation device 106, that is, if the model abnormality flag is "0" or the modeling error can not be calculated, It transfers to step 210.

  In step 210, the system error notification means 114 determines the possibility of occurrence of an estimated position error that may adversely affect the system within a certain period of time. If the system is adversely affected, the system status is output as that effect. Do.

  FIG. 11 shows the process flow of the system abnormality notifying means 114.

  First, in step 1101, from the temporary storage area 115, obtain the modeling error variance value corresponding to the highest vehicle state from the current vehicle state, the error variance value calculated by the internal model, and the probability of the current vehicle-like x state .

  Next, in step 1102, the minimum time until the estimated error d which can be calculated from the dispersion value obtained by adding the modeling error dispersion value to the error dispersion value of the internal model acquired in step 1101 is calculated. The estimation error can be calculated as in the following (Equation 21).

  The right side of the above equation represents an elliptic equation, and the amount by which the major axis extends in time units is determined by the error variance value Σ. Therefore, the minimum time until the estimation error exceeds the threshold can be determined by dividing the estimation error threshold by the amount by which the major axis extends in time units.

  Also, the time to this threshold is determined at design time, and this step is unnecessary if a fixed value is given.

  Next, in step 1103, it is determined whether the time until the estimated error exceeds the threshold is equal to or less than the threshold. If it is less than the threshold value, it is not a system abnormality, and there is no information to be notified, and the processing of the system abnormality notification unit 114 is ended.

  If the time until the estimation error exceeds the threshold is larger than the threshold, the process proceeds to step 1104 and it is determined whether the modeling error calculation impossible flag is “1”. This is “1” when the external model is not acquired at the processing step 1002 of the internal model error calculation means 113 and the error can not be calculated although there is an abnormality in the internal model. .

  If the modeling error can not be calculated, the process proceeds to step 1105. In this case, since the system itself of the position estimation device 106 may output an abnormal value, the system state is output to the outside of the position estimation device 106 as a system abnormality of the position estimation device 106.

  If the modeling error can be calculated, since the position estimation device 106 is operating normally, there is a possibility that the external system (position measurement means 101) is abnormal. Therefore, the system state is output to the outside as a system abnormality other than the position estimation device 106.

  When the output of the system state to the outside is completed, the processing of the system abnormality notifying unit 114 is ended. When the processing of the system abnormality notification unit 114 is completed, the processing of the position estimation device 106 is ended to prepare for the processing at the next activation.

  By the above series of operations, the abnormal state of the position estimation device 106 can be output from the system abnormality notification unit 114, and the user can determine whether the position estimation device 106 is abnormal or the external system is abnormal.

  There are various judgment methods of the abnormal value used in the model abnormality judging means 111, and it is also possible to use a method different from that used in the first embodiment. For example, in addition to the independence determination, it is possible to determine an abnormal value even in processing in which it is determined as abnormal when the difference between the internal model and the external model is equal to or more than a threshold. However, setting of the threshold value requires setting based on sufficient experience.

Reference Signs List 101 position measuring means, 102 attitude measuring means, 103 velocity measuring means, 104 acceleration measuring means, 105 angular velocity measuring means, 106 position estimating device, 107 external model calculating means, 108 internal model calculating means, 109 observation abnormality judging means, 110 probability Filter calculation means, 111 model abnormality determination means, 112 error series data table, 113 internal model error calculation means, 114 system abnormality notification means, 115 temporary storage area

Claims (5)

Internal model computing means for updating the position of the object from the motion parameters of the object;
External model computing means for calculating the position itself of the object from position measuring means for measuring the position of the object;
Stochastic filter calculation means for correcting the output position of the internal model by a probability filter using the result of the observation abnormality judgment means for judging the outliers of the position calculated by the external model based on the error variance value calculated by the internal model A position estimation device having
Model abnormality judging means for judging that the internal model is abnormal;
Internal model error calculation means for calculating an error of the internal model when there is an abnormality;
System abnormality notification means for judging the possibility of breakage of the position output by the position estimation device from the state of abnormality of the internal model and the external model, and outputting as abnormality of the system state,
A position estimation device which reports the possibility that the output position is broken by the system abnormality notifying means when the model abnormality judging means judges that the abnormality is made and the observation abnormality judging means makes the abnormality. In the position estimation device of claim 1,
The position where the observation abnormality determination means performs outlier determination by adding the error dispersion of the modeling error of the internal model calculated by the internal model error calculation means to the error dispersion used for outlier determination based on Mahalanobis distance Estimator. In the position estimation device of claim 1,
The model abnormality judging means sets a series of differences with the external model, which can be calculated by updating the position by the internal model, with the same series in a forward direction and a reverse direction, with the position output by the external model as an end point. A position estimation device that determines whether In the position estimation device of claim 1,
A position estimation apparatus which calculates the modeling error of the internal model from the difference between the position calculated by the external model and the estimated position of the position estimation apparatus. In the position estimation device of claim 1,
The system abnormality notifying means determines whether the external model is abnormal or the internal model is abnormal. If the external model is abnormal, the system abnormality other than the position estimation device or the internal model is abnormal A position estimation device that outputs that there is a possibility that a system abnormality of the position estimation device has occurred as a system state.

Images