JP3218441B2 - Filter device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a filter device having a Kalman filter, and more particularly to a filter device having high noise suppression and high responsiveness even when the number of observed state variables is small. It relates to the technology for obtaining it.

[0002]

2. Description of the Related Art In general, techniques for obtaining a moving average and techniques such as a Kalman filter are effective means for extracting a true signal by suppressing white noise.

In the former method for obtaining a moving average, the noise suppression degree and the responsiveness are in a exactly trade-off relationship. To increase the noise suppression degree, if the data amount of observation data is increased, the responsiveness is correspondingly increased. On the other hand, there is a problem that the noise suppression is degraded when trying to increase the responsiveness.

[0004] On the other hand, the latter Kalman filter forms a state model of a true signal generation process and generates a minimum signal based on observation data of state variables (for example, position, velocity, acceleration, etc.) obtained by various measuring instruments. It performs square estimation and has a mechanism to update the above state variables every unit time, so if a state variable can be obtained for each unit time, the estimation error can be reduced by accumulating the data. It becomes smaller gradually, has a high noise suppression degree, and has high responsiveness.

In this Kalman filter, as is well known, if measurement data Zm (t) of state variables (position, velocity, acceleration, etc.) can be obtained, an estimated value X (t) at a time t estimated at a certain time t is obtained. / T) is given by the following equation.

X (t / t) = X (t / t−1) + G (t) · Zr (t) (1) where X (t / t−1) = Ф (t) · X (t) −1 / t−1) (2) Zr (t) = Zm (t) −H (t) · X (t / t−1) (3) where X (t / t−1) is the time t X (t−1 / t−1) is the estimated value at time t−1 estimated at time t−1, G (t) is the filter gain,
r (t) is an observation residual, H (t) is an observation matrix, and Ф (t) is a transition matrix.

FIG. 3 shows a block diagram of a Kalman filter F that models the above equations (1) to (3). In the figure, 1 is a subtractor for calculating the observation residual Zr (t), 2
Is a multiplier of the filter gain G (t), 3 is an estimated value X (t / t)
, 4 is a delay circuit that delays the output of the adder 3 by a predetermined time and outputs the result, 5 is a multiplier for the transition matrix Ф (t), and 6 is a multiplier for the observation matrix H (t). It is a vessel.

[0008]

In the Kalman filter F having the configuration shown in FIG. 3, for example, when it is desired to obtain speed data as the estimated amount X (t / t) of the state variable, the data input sections in ₁ and in _{1 of} the Kalman filter are required. velocity and acceleration of the observed data Zm ₁ relative in ₂ (t), if Zm ₂ (t) are input, than the rate observed amount Zm ₁ of (t), obtained in this Kalman Filter F estimated The quantity X (t / t) has high noise suppression and high responsiveness.

For example, assuming a case where a ship stops by receiving a negative acceleration from a constant speed state, as shown in FIG. Change. Speed estimator of the Kalman filter F with this is changed as shown by reference numeral x ₁ in FIG. In addition,
In the moving average, in the case of obtaining a signal output having a Kalman filter similar to the noise suppression degree, the output response as shown by reference numeral x ₃ is deteriorated.

By the way, the Kalman filter F includes only the observation data Zm (t) necessary for obtaining the estimated amount X (t / t).
Is not always input. For example, some ships are equipped with only a speedometer and not an accelerometer. In this case, the velocity observation data Zm ₁ (t) is input only to one of the data input sections in ₁ of the Kalman filter F, and the acceleration estimation performance at the observation update time deteriorates. Gets worse. In particular, a problem arises when it is desired to increase noise suppression because the speed observation noise is large. For example, in the example shown in FIG. 4, the rate estimator of the Kalman filter F is as shown by reference numeral x ₂ in FIG. In other words, the characteristics of the Kalman filter F cannot be sufficiently exhibited.

SUMMARY OF THE INVENTION The present invention has been made in view of such a problem, and has a high noise suppression degree and a high response which the Kalman filter originally has even when the number of observed state variables input to the Kalman filter is small. It is an object of the present invention to exhibit the property of sex.

[0012]

SUMMARY OF THE INVENTION The present invention has been made to solve the above problems, and has the following structure.

That is, the filter device of the present invention includes a Kalman filter, and an observation data storage means for storing a plurality of observation data obtained from a speedometer of a ship, in front of a data input section of the Kalman filter; Determine the number of observation data points to be stored in the storage means
Preset the number of data points from the sampling point decider
A counter to be input, and a counter stored in the observation data storage means.
A least-squares regression curve is calculated based on the
Of the velocity obtained by the least-squares regression curve
Data and higher-order derivatives than the estimated data at this speed
Control of the estimated observation data of the acceleration from the counter
A switching circuit that switches contacts only when a signal is given
And a least-squares calculating means for outputting the result to the Kalman filter.

[0014]

In the above arrangement, a plurality of observation data obtained by a measuring instrument such as a speedometer is stored in the observation data storage means. Subsequently, the least-squares calculating means obtains a least-squares regression curve based on these observation data, and calculates estimated observation data obtained by the least-squares regression curve and estimated observation data of a higher-order derivative than the estimated observation data. Decide each. Then, these estimated observation data are output to the Kalman filter. Therefore, since the Kalman filter is input after the shortage of the parameter of the state variable has been compensated, an output of an estimator with high noise suppression and high responsiveness is obtained.

[0015]

FIG. 1 is a block diagram of a filter device according to an embodiment of the present invention.

In FIG. 1, reference numeral F denotes a Kalman filter. Since the configuration of this Kalman filter F is the same as that shown in FIG. 3, the same reference numerals are given to the portions corresponding to FIG. 3, and the detailed description will be omitted.

The feature of the filter device of this embodiment lies in the components provided before the data input units in ₁ and in ₂ of the Kalman filter F. That is, in FIG. 1, M is a memory as observation data storage means. In this example, the memory M is a plurality of observation data (here, velocity data) Zm ₀ (t) obtained by a speedometer (not shown). (Ns in this example) are stored in chronological order, and the observation data Zm ₀ (t) is stored in synchronization with a write permission pulse R given from a CPU (not shown). Further, in a state where only Ns pieces of observation data Zm ₀ (t) are stored, the oldest observation data is discarded every time one new observation data Zm ₀ (t) is input as in a shift register. Configuration.

L is a least-squares arithmetic unit which obtains a least-squares regression curve based on the Ns observation data stored in the memory M, and estimates estimated data Zm ₁ (t) obtained by the least-squares regression curve. And output the estimated observation data Zm ₂ (t) of a higher order derivative (acceleration in this example) than the estimated observation data.

S is a memory M based on the data of the observation residual Zr (t) output from the adder 1 of the Kalman filter F.
Is a data point number determiner as a data storage amount changing means for changing the data point number Ns of observation data stored in the data point.
This data point number determiner S is provided for the following reason.

Generally, the observation residual Zr (t) obtained by the Kalman filter F is only an observation noise component included in observation data when it reaches a steady state. On the other hand, in order to obtain estimated acceleration data Zm ₂ (t) of pseudo acceleration,
If the number of data points Ns is set to be excessively large, a large time delay occurs in the speed and acceleration obtained by the least squares regression estimation. As a result, the effects of the present invention cannot be expected. Therefore, the number of data points Ns needs to be set to an appropriate value so as not to be excessively large. If the appropriate Ns is set in this way, the problem caused by the delay caused here is that the velocity and the acceleration in which the observation noise is suppressed by the least-squares estimation can be obtained. Therefore, the responsiveness due to the fact that the observation noise parameter R of the Kalman filter can be set small is obtained. The benefits of improvement will outweigh the benefits. Therefore, if the number Ns of data points of observation data stored in the memory M is changed to an optimum value according to the magnitude of the observation residual Zr (t), appropriate responsiveness can always be obtained according to the magnitude of observation noise. Will be.

In C, the number of data points Ns from the sample point number determiner S is preset and input, and the number of data points Ns is decremented in accordance with the write permission pulse R, and the memory M, the least square calculator L, and Is a counter that outputs a control signal to the switching circuit K.

K is a switching circuit for switching the connection from the normally closed contact (black) to the switching contact (white) only when a control signal from the counter C is given.

Next, the operation of the filter device having the above configuration will be described. In this embodiment, only the speed data from the speedometer (not shown) is stored in the filter device as the observation data Zm.
It shall be input as ₀ (t).

First, in a state where the observation data Zm ₀ (t) of the predetermined number of data points Ns has not yet been stored in the memory M, the counter C permits writing of the value of the number of data points Nm set by the data point number setting device S. Each time the pulse R is input, the count is in the process of being decremented. Therefore, no control signal is output from the counter C. Therefore, the switching circuit K is not connected to the least square calculator L, and the observation data (speed data) Zm ₀ (t) from the speedometer is input to the data input unit in ₁ of the Kalman filter F via the switching circuit K. . Therefore, the estimated speed X (t) obtained by the Kalman filter F
/ T) is still inadequate.

However, once the observation data Zm ₀ (t) of the predetermined number of sampling points Ns is stored in the memory M in a time series, the write permission pulse R
Is input from the counter C to the memory M, the least-squares calculator L, and the switching circuit K, respectively.

The memory M stores the latest observation data Zm ₀ (t) inputted each time the write permission pulse R is inputted, and discards the oldest observation data, while discarding the oldest observation data. In response to the control signal, the Ns observation data (including the latest one) stored therein is transferred to the least-square calculator L in the next stage.

The least-squares calculator L calculates Ns observation data Zm ₀ (t) transferred from the memory M,
As shown in FIG. 2, a least squares regression curve is obtained. For example, to find the least squares regression curve as a linear equation,
The following equation is obtained.

Y (t) = Zm ₂ (t) · t + Zm ₁ (t) (4) Then, the least-squares calculator L uses the coefficient Zm ₁ (t) in the equation (4) as estimated velocity observation data. also the estimated observation data Zm ₁ (t) higher-order derivative of Zm ₂ than (t) respectively determined as the estimated observation data of the acceleration, these data Zm ₁ (t), Zm ₂ a (t) both Output to the switching circuit K.

The switching circuit K stores the observation data Zm _{0 in the} memory M.
(t) is stored for a predetermined number of data points Ns, and every time a control signal is output from the counter C, the output of the least-squares arithmetic unit L is selected. The above estimated observation data Zm ₁ (t) and Zm ₂ (t)
Are input to the data input sections in ₁ and in ₂ of the Kalman filter F via the switching circuit K, respectively. Therefore, in reality, even when only the speed observation data Zm ₀ (t) is measured, the estimated acceleration data Zm ₁ (t) and the estimated acceleration data Zm ₁ (t) are supplied to the Kalman filter F.
₂ (t) is input, so to say, the shortfall of the state variable parameter has been compensated, and the Kalman filter F
The data X (t / t) of the speed estimation amount obtained in (1) has a high noise suppression degree and a high responsiveness.

For example, as shown in FIG. 4, when the ship stops by receiving a negative acceleration from the constant speed state, the estimated speed X (t / t) of the Kalman filter F in this embodiment.
, As the curve indicated by symbol x ₀ in the figure, it is an approximation to the curve x ₁ of estimators When entering observation data speed and acceleration are both directly Kalman filter F, only the observed data rate Kalman filter and excellent responsiveness than the estimated amount of the curve x ₂ for entering into F.

Since the data of the observation residual Zr (t) output from the subtractor 1 of the Kalman filter F is input to the data point determiner S, the data point determiner S outputs the observation residual Zr (t). Based on t), the number of data points Ns given to the counter C is changed. In the case of this example, the observation residual Zr
Upper and lower thresholds are provided stepwise according to the magnitude of (t), and the observation residual Zr is set within each threshold.
Since the number of data points Ns corresponding to the magnitude of (t) is set in advance, if the range of the upper and lower thresholds includes the observation residual Zr (t), the optimum number of data points Ns
Is output to the counter S. In particular, the observation data Zm ₀ (t)
When the observation noise is large, the number of data points Ns is large, so that the responsiveness is deteriorated to some extent. However, the noise suppression degree and the response performance can be improved as compared with the general moving average method.

[0032]

According to the present invention, since the Kalman filter is input after the shortage of the parameters of the state variables is always compensated for, the output of the estimator with high noise suppression and high responsiveness is output. Characteristics will be exhibited.

In particular, since the estimated observation data obtained by the least squares regression is obtained, compared with the case where the observation data is directly input to the Kalman filter, the observation amount in which the observation noise is further reduced is input to the Kalman filter. , The parameters of the observation noise can be reduced, and high responsiveness is maintained.

Further, since the number of storage points of observation data is changed according to the magnitude of the observation residual, an appropriate response can always be obtained depending on the level of observation noise.

The present invention is effective when applied to, for example, a speedometer using a Doppler sonar or the like.

[Brief description of the drawings]

FIG. 1 is a block diagram of a filter device according to an embodiment of the present invention.

FIG. 2 is an explanatory diagram of a method of determining a least squares regression curve by a least squares calculating means.

FIG. 3 is a block diagram of a general Kalman filter.

FIG. 4 is a characteristic diagram showing a change over time in a speed estimation amount of a Kalman filter when a ship receives a negative acceleration from a constant speed state and stops.

[Explanation of symbols]

F: Kalman filter, in ₁ , in ₂ ... data input section, M ...
Observation data storage means (memory), L ... Least square calculation means
(Least square calculator), S: data storage amount changing means (sample point number determiner), C: counter.

Claims (2)

(57) [Claims] A Kalman filter is provided, and a Kalman filter is provided in front of a data input unit with a speedometer of a ship.
Observation data storage means for storing the obtained plurality of observation data; and observation data storage means for storing the observation data in the observation data storage means .
Data from the sampling point determiner that determines the number of data points
A counter for presetting the number of data points and the observation data
Data based on the speed observation data stored in the data storage means.
Find the least squares regression curve and use this least squares regression curve
Obtained estimated speed data and estimated speed
Estimated observation data of acceleration of higher derivative than observation data
Only when a control signal from the counter is given.
A filter device , comprising: a least-squares calculating means for outputting to the Kalman filter via a switching circuit for switching a contact . 2. The filter device according to claim 1, further comprising a data storage amount changing unit that changes a data storage amount in the observation data storage unit based on ship speed observation residual data in the Kalman filter.