JP3205758B2 - 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 of calculating a moving average, the degree of noise suppression and responsiveness are exactly in a trade-off relationship. In order to increase the degree of noise suppression, if the data amount of observed values 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 obtains a minimum based on observation values 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 state variables are obtained every unit time, accumulate the data of the observation values As a result, the estimation error gradually decreases, and the noise has a higher degree of noise suppression and a higher responsiveness than when a moving average is obtained.

In this Kalman filter, as is well known, the observed values Zm of state variables (position, velocity, acceleration, etc.) are known.
If (t) is obtained, an estimated value (hereinafter referred to as the most probable value) X (t / t) at the same time t estimated at a certain time 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 Next time t estimated by -1
, X (t−1 / t−)
1) is the most probable value at the same time t-1 estimated at the time t-1, G (t) is the filter gain, Zr (t) is the observation residual, H (t)
Is the observation matrix, and Ф (t) is the transition matrix.

FIG. 3 shows a block diagram of a Kalman filter F that models the above equations (1) to (3). In the figure, reference numeral 1 denotes an observed estimated value H (t) .X (t / t-) obtained by a third multiplier 6, which will be described later, from the input observed value Zm (t) of the state variable.
1) a subtractor for calculating the observation residual Zr (t) by subtracting 1), 2 a first multiplier for multiplying the observation residual Zr (t) by the filter gain G (t), 3 a first multiplier 2 Output G (t) · Zr (t)
An adder that calculates the most probable value X (t / t) by adding the predicted value X (t / t-1) obtained by a second multiplier 5 described later to
Is a delay circuit that delays and outputs the output X (t / t) of the adder 3 by a unit time, and 5 is a transition matrix that converts the most probable value X (t−1 / t−1) output from the delay circuit 4 to Ф The second multiplier 6 calculates the predicted value X (t / t-1) by multiplying by (t), and the second multiplier 6 multiplies the output X (t / t-1) of the second multiplier 5 by the observation matrix H (t). To calculate the observation estimated value H (t) .X (t / t-1)
It is a multiplier.

In the Kalman filter F having the configuration shown in FIG. 3, for example, when it is desired to obtain velocity data as the most probable value X (t / t) of the state variable, the data input sections in ₁ and in _{2 of the} Kalman filter F are provided. If not only the observed value Zm ₁ (t) of the velocity but also the observed value Zm ₂ (t) of the acceleration of the higher-order derivative of this velocity is input, a highly accurate acceleration estimation can be obtained,
The speed prediction value can be correctly estimated. Therefore, characteristics of high noise suppression and high response can be obtained.

For example, assuming a case where the ship stops by receiving a negative acceleration from a constant speed state, as shown in FIG. 4, the observed speed value Zm (t) is represented by a curve z shown in FIG. Changes with time. Most probable value X of speed of the Kalman filter F associated therewith (t / t) changes as shown by reference numeral x ₁ in FIG. It should be noted that 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.

[0010]

Incidentally, it is not always the case that the observation values Zm (t) necessary for obtaining the most probable value X (t / t) are always input to the Kalman filter F. For example, some ships are equipped with only a speedometer and not an accelerometer. In this case, the velocity observation value Zm is input to one data input section in ₁ of the Kalman filter F.
Only ₁ (t) is input. In this case, the accuracy of estimating the acceleration state variable deteriorates, and a high-precision X (t / t) speed component cannot be obtained. As a result, the responsiveness deteriorates. For example, in the example shown in FIG. 4, the most probable value X of speed of the Kalman filter F (t / t) is as shown by reference numeral x ₂ in FIG.

As described above, in order to improve the responsiveness of the estimated output of the most probable value X (t / t) when the number of parameters of the observed value is small, in the prior art, the filter in the above equation (1) is used. Attempts have also been made to intentionally change the magnitude of the gain G (t) so as to obtain appropriate filter characteristics.

That is, the magnitude of the filter gain G (t) in the Kalman filter F is related to the magnitude of the observation noise R contained in the actual observation Zm (t) and the magnitude of the system noise Q generated in the signal processing system. For example, when the observation noise R included in the actual observation value Zm (t) is large,
Accordingly, the filter gain G (t) decreases. As a result, as can be seen from equation (1), the weighting on the observed value Zm (t) is reduced, and a high noise suppression degree is obtained accordingly. However, since the weighting of the observation value Zm (t) is reduced, the responsiveness is adversely degraded. Therefore,
In this case, if the filter gain G (t) is adjusted by intentionally applying an external signal or the like to increase the filter gain G (t), the weighting of the observed value Zm (t) is increased, and the response is improved. However, on the contrary, the degree of noise suppression is sacrificed to some extent.

In other words, in a situation where the observation noise R included in the actual observation value is large, simply taking a measure of adjusting the filter gain G (t) as in the related art does not provide a high noise suppression degree. In addition, it has been difficult to obtain a filter characteristic having high responsiveness.

The present invention has been made in view of such a problem, and has a high noise suppression degree even when the number of state variables input to the Kalman filter is small and the observation noise is large. Another object of the present invention is to provide a filter with high response characteristics.

[0015]

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 a smoothing processing unit for smoothing the magnitude of the observation residual Zr (t) of the Kalman filter, and a smoothed output Zrf ( a correction value generation unit that generates a correction value Xc (t) corresponding to the magnitude of t) and corrects the correction value Xc (t) at a cycle corresponding to the magnitude of the smoothed output Zrf (t); The correction value Xc (t) from the correction value generator is used as the predicted value X (t / t-1) of the state variable of the Kalman filter.
And an adder for adding the value to

[0017]

In the above configuration, an observation value obtained by a measuring instrument such as a speedometer is input to a Kalman filter. In response, the Kalman filter obtains the observation residual Zr (t) based on the equation (3). The magnitude of the observation residual Zr (t) is smoothed in the smoothing processing unit, Is input to the correction value generator of the next stage. The correction value generator generates a correction value Xc (t) corresponding to the magnitude of the smoothed output Zrf (t), and generates the correction value Xc (t).
Is output at a cycle corresponding to the magnitude of the smoothed output Zrf (t).
Then, the correction value Xc (t) from the correction value generating unit is added to the predicted value X (t / t) of the state variable of the Kalman filter by the adding unit.
-1).

That is, in the present invention, since the conventional adjustment of the filter gain G (t) is not performed at all, R is set to an appropriate value such as when the observation noise R included in the observation value is large. Set to ensure high noise suppression. Then, a correction amount corresponding to the magnitude of the observation residual is created and added to the desired estimated output X (t / t). In addition, according to the magnitude of the smoothed output of the observation residual Zr (t), the predicted value X (t
/ T-1) is directly corrected for each predetermined period, so that high responsiveness can be obtained even when the weight of the observation value Zm (t) is small.

[0019]

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 basic configuration of the Kalman filter F is the same as that shown in FIG. 3, portions corresponding to FIG. 3 are denoted by the same reference numerals and detailed description thereof will be omitted.

The filter apparatus of this embodiment is characterized by a smoothing section 7 for smoothing the magnitude of the observation residual Zr (t) obtained by the subtractor 1 of the Kalman filter F, and a smoothing section 7
Correction value Xc (t) corresponding to the magnitude of the smoothed output Zrf (t) from
And the correction value Xc (t) is converted to a smoothed output Zrf.
A correction value generator 8 that changes the correction cycle in accordance with the magnitude of (t), and a state variable obtained by the second multiplier 5 of the Kalman filter F using the correction value Xc (t) from the correction value generator 8. And an adder 12 that adds the predicted value X (t / t-1) to the predicted value X (t / t-1).

The smoothing processing section 7 responds to the control pulse p output from the correction value generating section 8 when the smoothed output Zrf (t) exceeds a preset threshold value ± Qth. The smoothed output Zrf (t) is configured to be reset. The correction value generation unit 8 includes a comparison processing unit 9, a surplus correction unit 10, and a correction value addition unit 11.

The comparison processing section 9 compares the smoothed output Zrf (t) from the smoothing processing section 7 with a predetermined threshold ± Qth, and the smoothed output Zrf (t) exceeds the threshold ± Qth. In this case, the control pulse p is output in response to this,
A magnitude correction value Xc ₁ (t) based on the following equation (4) is generated.

Zrf (t) ≧ + Qth: Xc ₁ (t) = + kc · Qth Zrf (t) ≦ −Qth: Xc ₁ (t) = − kc · Qth | Zrf (t) | <Qth: Xc ₁ (t ) = 0 (where kc is a constant) (4) The surplus correction unit 10 counts the control pulses p from the comparison processing unit 9 and responds to the number n of the control pulses p based on the following equation (5). A surplus correction value Xc ₂ (t) is generated, and when the polarity of the correction value Xc ₁ (t) from the comparison processing unit 9 is inverted, the signal output Xc ₂ (t) is reset. I have.

Xc ₂ (t) = kd · (n−1) (where kd is a constant) (5) Further, the correction value adding section 11 calculates the correction value Xc ₁ (t) from the comparison processing section 9 as follows: The surplus correction value Xc ₂ (t) from the surplus correction unit 10 is added and output to the addition unit 12. As described above, the reset function in the averaging unit 7 and the correction value X
By providing the _second correction value Xc ₂ (t) whose magnitude changes in proportion to the frequency of occurrence of the same polarity correction amount of c ₁ (t), high responsiveness can be obtained even when the target value changes with a large acceleration. Follow-up becomes possible.

Next, the operation of the filter device having the above configuration will be described with reference to a time chart shown in FIG.
In this example, it is assumed that only speed data is input as an observation value Zm (t) from a speedometer (not shown) to the filter device, and that the speed is increased by receiving a positive acceleration. It shall be.

In the case where the speed is increased by receiving a positive acceleration, the observation value Zm (t) obtained accordingly is also shown in FIG.
As shown by the broken line in (a), it gradually increases with time. Then, the observed value Zm (t) is input to the subtractor 1 via the data input unit in of the Kalman filter F.

The subtractor 1 calculates the input observation value Zm (t)
Is subtracted from the output H (t) · X ′ (t / t−1) of the third multiplier 6 to calculate the observation residual Zr (t). This observation residual Z
The magnitude of r (t) is only the observation noise R included in the observation value Zm (t) when, for example, the ship is traveling at a constant speed. However, when the speed is increased by receiving a positive acceleration as in this example, a response delay occurs in the estimation calculation of the final value X (t / t). As shown by the broken line in FIG. 2B, the observation residual Zr (t) also gradually increases. The observation residual Zr having such a tendency
(t) is applied to the first multiplier 2 at the next stage and is also input to the smoothing processing unit 7.

The smoothing processing unit 7 smoothes the observation residual Zr (t), and supplies the smoothed output Zrf (t) to the comparison processing unit 9 at the next stage. The comparison processing unit 9 compares the magnitude of the smoothed output Zrf (t) with a preset threshold value ± Qth. Then, a magnitude correction value Xc ₁ (t) based on the above equation (4) proportional to the magnitude of the threshold value Qth is generated. Here, when the magnitude of the observation residual Zr (t) is larger than the threshold value Qth (|
For Zr (t) |> | Qth |), the smoothed output Zrf (t) exceeds the threshold value Qth. At this time, the comparison processing unit 9 outputs the control pulse p and the correction value Xc _1. (t) (here, kc · Qth) is generated (see FIG. 2C). Then, the smoothed output Zrf (t) of the smoothing processing unit 7 is reset by the control pulse p output from the comparison processing unit 9 (FIG. 2).
(See (b)).

The control pulse p is supplied to the surplus corrector 10
In addition, the surplus correction unit 10 calculates the control pulse p
And generates a surplus correction value Xc ₂ (t) corresponding to the number n of the control pulses p based on the above equation (5) (see FIG. 2C).

Then, the correction value Xc from the comparison processing section 9 is obtained.
_{Since 1} (t) and the surplus correction value Xc ₂ (t) from the surplus correction unit 10 are given to the correction value adding unit 11, respectively, the correction value adding unit 11 outputs the two correction values Xc ₁ (t) and Xc ₂ (t) is added, and the correction value Xc (t) (= Xc ₁ (t) + Xc ₂ (t)) obtained by the addition is further output to the adding unit 12. The adder 12 adds the correction value Xc (t) from the correction value generator 8 to the predicted value X (t / t-1) of the state variable obtained by the second multiplier 5 of the Kalman filter F.

Therefore, assuming that the corrected predicted value output from the adder 12 is X '(t / t-1), X' (t / t-1) = X (t / t-1) + Xc (t) (6) The output X ′ (t / t−1) of the adder 12 is applied to the third multiplier 6 and the adder 3 constituting the Kalman filter F.

The third multiplier 6 adds the observation matrix H (t) to the corrected predicted value X ′ (t / t−1) obtained by the adding unit 12.
To calculate the observed estimated value H (t) · X ′ (t / t−1),
This observation estimated value H (t) · X ′ (t / t−1) is given to the subtractor 1.

Since the observed estimated value H (t) .X '(t / t-1) includes the term of the correction value Xc (t) as can be seen from the relationship of the equation (6). , The observation residual Zr (t) output from the subtracter 1 is, as shown in FIG.
It is reduced each time X '(t / t-1) is added. However, when the magnitude of the observation residual Zr (t) is still larger than the threshold value Qth in the comparison processing unit 9 (| Zr (t) | ≫ | Qth |), the output Zrf ( Since t) increases again after the reset and reaches the threshold value Qth, the operation of resetting the output Zrf (t) of the smoothing processing unit 7 again is repeated. At this time, the time when the smoothed output Zrf (t) reaches the threshold value Qth is also characterized in that it changes according to the magnitude of the observation residual Zr (t). That is, the larger the observation residual Zr (t) is, the more frequently the correction cycle is increased, which contributes to the improvement of the response. Even if the observation residual Zr (t) is temporarily reduced, if the correction cannot be completed, the correction value Xc ₁ (t) output from the comparison processing unit 9 has the same polarity. The output surplus correction value Xc ₂ (t) gradually increases as can be seen from the relationship of equation (5). That is, as shown in this example, since the velocity is increased by receiving a positive acceleration, the observation residual Zr (t)
Is gradually increased, the correction value Xc (t) is accordingly increased. Therefore, the Kalman filter F
The most probable value X (t / t) (shown by a solid line in FIG. 2A) obtained by the adder 3 of FIG. (Indicated by a two-dot chain line), the observed value Zm (t) approaches within a short time, and the responsiveness is increased.

As described above, according to the present invention, since the filter gain G (t) is not intentionally adjusted at all as in the prior art, when the observation noise R included in the observation value Zm (t) is large, Can set R according to the amount of noise. That is, as a result, the filter gain G (t)
Becomes smaller with itself, and a high noise suppression degree can be maintained.
In addition, the predicted value X (t / t-1) output from the second multiplier 5 is corrected in accordance with the magnitude of the observation residual Zr (t).
Is directly corrected by the correction value Xc (t) from
Since the output cycle of the correction value Xc (t) becomes shorter as the value of r (t) becomes larger, the calculation of the most probable value X (t / t) obtained by the adder 3 becomes short, and high responsiveness is obtained. Become like

For example, in FIG. 4 described above, when the ship stops by receiving a negative acceleration from the constant speed state, the output X (t / t) of the Kalman filter F in this embodiment.
, As the curve indicated by symbol x ₀ in the figure, it is assumed that approximates the estimate of the curve x ₁ for entering observations of velocity and acceleration are both directly Kalman filter F, and is excellent in responsiveness Become.

In the above embodiment, for the sake of simplicity, only the velocity data is input to the filter device as the state variable as the observation value Zm (t). The present invention can also be applied to the case where the state variables of differentiation (for example, acceleration) are simultaneously obtained.
This case can be dealt with by adding the smoothing processing unit 7 and the correction value generation unit 8 in parallel.

[0038]

According to the present invention, a filter having a high noise suppression degree and a high responsiveness even when the number of observed state variables input to the Kalman filter is small and the observed noise is large. Characteristics will be exhibited.

The present invention is particularly effective when applied to 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 a time chart for explaining the operation of the filter device of FIG. 1 in the case where the speed of a ship is increased by receiving a positive acceleration.

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, 1 ... Subtractor, 2 ... First multiplier,
3 adder, 4 delay circuit, 5 second multiplier, 6 third
Multiplier, 7: smoothing processing unit, 8: correction value generation unit, 12: addition unit.

Claims (1)

(57) [Claims] 1. A filter device having a Kalman filter, comprising: a smoothing section for smoothing the magnitude of an observation residual Zr (t) of the Kalman filter; and a magnitude of a smoothed output Zrf (t) from the smoothing section. Generates a correction value Xc (t) corresponding to the correction value Xc (t).
A correction value generator that outputs the correction value Xc (t) from the correction value generator at a cycle corresponding to the magnitude of the smoothed output Zrf (t). -1) an addition unit for adding -1).