JP2019158838A - Estimating arithmetic unit and estimating arithmetic program - Google Patents

Abstract

To provide an estimating arithmetic unit and an estimating arithmetic program which reduces processing load while maintaining accuracy.SOLUTION: An estimating arithmetic unit includes: a α-β filter processing section, a Kalman filter processing section, an extended Kalman filter processing section, and a first selection section. The α-β filter processing section derives estimated position coordinates, an estimated speed, a turning radius for a target by performing α-β filtering observation data of the target acquired by a radar device. The Kalman filter processing section and the extended Kalman filter processing section derives the estimated position coordinates and the estimated speed for the target from the observation data acquired by the radar device. The first selection section causes the Kalman filter processing section to perform Kalman filter processing when the turning radius derived by the α-β filter processing section is larger than a threshold, and causes the extended Kalman filter processing section to perform extended kalman filter processing when the turning radius derived by the α-β filter processing section is smaller than the threshold.SELECTED DRAWING: Figure 1

Description

  Embodiments described herein relate generally to an estimation calculation device and an estimation calculation program.

  A technique for observing a moving target with a radar or the like and predicting a future position of the target with a prediction filter is disclosed.

  In the conventional technology, depending on the estimation accuracy of the filter, there is a possibility that a certain estimation accuracy may not be satisfied. There was a case. In addition, when the target repeatedly turns and goes straight, data of a large number of observation points may be necessary to improve the accuracy of speed estimation of the target.

Japanese Patent Publication No. 55-95177

  The problem to be solved by the present invention is to provide an estimation calculation device and an estimation calculation program capable of reducing processing load while maintaining accuracy.

  The estimation calculation device according to the embodiment includes an α-β filter processing unit, a Kalman filter processing unit, an extended Kalman filter processing unit, and a first selection unit. The α-β filter processing unit performs an α-β filter on the observation data of the target acquired from the radar device to derive the estimated position coordinates, the estimated speed, and the turning radius of the target. The Kalman filter processing unit performs Kalman filter processing on the observation data acquired from the radar apparatus to derive the estimated position coordinates and estimated speed of the target. The extended Kalman filter processing unit performs an extended Kalman filter process on the observation data acquired from the radar device to derive the estimated position coordinates and the estimated speed of the target. When the turning radius derived by the α-β filter processing unit is larger than the threshold, the first selection unit causes the Kalman filter processing unit to perform Kalman filtering, and when the turning radius derived by the α-β filter processing unit is smaller than the threshold, The extended Kalman filter processing unit is caused to perform extended Kalman filter processing.

The function block diagram which shows the estimation calculating apparatus 100. FIG. The figure which shows an example of the locus | trajectory of the target object OB. 4 is a timing chart illustrating an example of selection processing by a first selection unit 130 and a second selection unit 160.

  Hereinafter, an estimation calculation device and an estimation calculation program according to embodiments will be described with reference to the drawings.

  FIG. 1 is a diagram illustrating a functional configuration of the estimation arithmetic device 100 of the present embodiment. The estimation calculation device 100 includes, for example, an acquisition unit 110, an α-β filter processing unit 120, a first selection unit 130, a Kalman filter processing unit 140, an extended Kalman filter processing unit 150, and a second selection unit 160. . These components are realized, for example, when a hardware processor such as a CPU executes a program (software). In addition, some or all of these components include hardware (circuits) such as LSI (Large Scale Integration), ASIC (Application Specific Integrated Circuit), FPGA (Field-Programmable Gate Array), and GPU (Graphics Processing Unit). Part (including circuit)), or may be realized by cooperation of software and hardware.

  The α-β filter processing unit 120 performs an α-β filter to derive an estimated position coordinate, an estimated speed, and a turning radius R of the target OB. The target object OB is, for example, a flying object that flies mainly in a straight orbit orbit. The Kalman filter processing unit 140 performs Kalman filter processing on the observation data acquired from the radar device on the premise that the target OB moves in a straight path, and derives the estimated position coordinates and the estimated speed of the target OB. . The extended Kalman filter processing unit 150 performs an extended Kalman filter process on the observation data acquired from the radar device on the assumption that the target OB moves in a turning trajectory, and obtains the estimated position coordinates and the estimated speed of the target OB. To derive.

  The estimation calculation device 100 derives the estimated speed and estimated position coordinates of the target OB based on the observation data from the radar device 200 that observes the target OB, and outputs the derivation result to the display device 300.

  The acquisition unit 110 acquires observation data from the radar apparatus 200 and outputs the observation data to the α-β filter processing unit 120. The α-β filter processing unit 120 performs α-β filter processing on observation data at at least three arbitrary points in the observation data at a predetermined operation interval, and estimates the estimated position coordinates of the target OB. The speed and the turning radius R are derived. The α-β filter processing unit 120 outputs the filter processing result to the first selection unit 130 and the second selection unit 160.

  The first selection unit 130 outputs observation data from the radar device 200 to either the Kalman filter processing unit 140 or the extended Kalman filter processing unit 150 based on the turning radius R input from the α-β filter processing unit 120. To process. More specifically, when the turning radius R derived by the α-β filter processing unit 120 is equal to or greater than the threshold, the first selection unit 130 causes the Kalman filter processing unit 140 to perform Kalman filter processing, and the α-β filter processing unit When the turning radius R derived by 120 is less than the threshold value, the extended Kalman filter processing unit 150 is caused to perform the extended Kalman filter processing. Thus, the first selection unit 130 switches the processing unit that derives the estimated position coordinates and the estimated speed of the target OB based on the turning radius R.

  The Kalman filter processing unit 140 performs a straight-ahead model Kalman filter process on the observation data from the radar apparatus 200, and derives an estimated position coordinate and an estimated speed of the target OB.

  The extended Kalman filter processing unit 150 executes the extended Kalman filter processing of the turning model on the observation data from the radar apparatus 200, and derives the estimated position coordinates and the estimated speed of the target OB.

  The second selection unit 160 uses the derivation result or combination of any one of the α-β filter processing unit 120, the Kalman filter processing unit 140, and the extended Kalman filter processing unit 150 as an estimation calculation result of the estimation calculation device 100 to the display device 300. Output.

  Hereinafter, the principle of processing performed by the Kalman filter processing unit 140 and the extended Kalman filter processing unit 150 will be briefly described.

  The Kalman filter processing unit 140 applies the movement of the target OB to the straight-ahead model, and derives the estimated position coordinates and the estimated speed of the target OB. The state space model included in the Kalman filter processing unit 140 is expressed by the following equations (1) and (2).

Here, x (k) is the state vector of the target OB at the observation time t _k , A and B are the transition matrix and drive matrix from the observation time t _k1 to t _{k + 1} , and u (k) is the target at the observation time t _k . Control input vector of the object OB, v (k) is a mean 0 at the observation time t _k and a system noise vector according to the normal distribution of the covariance matrix Q (k), y (k) is an observation vector at the observation time t _k , C is the observation matrix at t _{k + 1} from the measurement time t _k1, w (k) is the average a 0 at the measurement time t _k, is the observation noise vector follows a normal distribution of the covariance matrix R (k).

  The Kalman filter processing unit 140 calculates the state estimation value by alternately performing the prediction steps represented by the equations (3) and (4) and the update steps represented by the equations (5) to (7).

In Equation (3) calculates the pre-state estimation value at the measurement time t _k, the formula (4), calculates the advance error covariance matrix at the measurement time t _k.

  In Equation (5), a Kalman gain matrix is calculated, in Equation (6), a state estimation value is calculated, and in Equation (7), a posterior error covariance matrix is calculated.

  The Kalman filter processing unit 140 can quantitatively determine how much reliability the obtained state estimated value has based on the error covariance matrix (P (k)) representing the accuracy of the state estimated value.

  The extended Kalman filter processing unit 150 applies the motion of the target OB to the turning model, and derives the estimated position coordinates and the estimated speed of the target OB. The state space model included in the extended Kalman filter processing unit 150 is expressed by the following equations (8) and (9).

  Here, f (x (k), u (k)) is a nonlinear function of x (k) and u (k), and h (x (k)) is a nonlinear function of x (k).

  The extended Kalman filter processing unit 150 alternately performs prediction steps represented by equations (10) to (13) and update steps represented by equations (14) to (16) using the following time update equation. Thus, the state estimated value is calculated.

  In Equation (10), a prior state estimated value is calculated, in Equations (11) and (12), linear approximation is performed, and in Equation (13), a prior error covariance matrix is calculated.

  In Equation (14), a Kalman gain matrix is calculated, in Equation (15), a state estimation value is calculated, and in Equation (16), a posterior error covariance matrix is calculated.

  Like the Kalman filter processing unit 140, the extended Kalman filter processing unit 150 determines how reliable the obtained state estimation value is based on the error covariance matrix (P (k)) representing the accuracy of the state estimation value. It can be judged quantitatively.

Hereinafter, a derivation example of the turning radius R of the target OB of the α-β filter processing unit 120 will be described with reference to FIG. FIG. 2 is a diagram illustrating an example of the trajectory of the target OB. P _(i) , p _(i-1) , and p _(i-2) in FIG. 2 indicate the position coordinates of three consecutive observation points of the target OB output from the radar apparatus 200. Note that p _(i) indicates a three-dimensional position coordinate at time t _(i) . For example, p _(i) = (x _{t (i)} , y _{t (i)} , z _{t (i)} ). It can be shown as follows. The α-β filter processing unit 120 calculates the turning radius of the target OB from the three observation points p _(i) , p _(i-1) , and p _(i-2) based on, for example, Expression (17). R and the center point C of the turning circle (sphere) of the target OB are derived. In the equation, || · || represents the Euclidean norm of the matrix, and det (a, b) represents the outer product of the vectors a and b.

  The α-β filter processing unit 120 outputs the derived turning radius R of the target OB to the first selection unit 130.

  The first selection unit 130 detects the movement state of the target OB based on the turning radius R of the target OB derived by the α-β filter processing unit 120, and determines whether the target OB is in a turning state or goes straight. It is determined whether it is in a state. For example, when the turning radius R of the target OB derived by the α-β filter processing unit 120 is equal to or greater than a preset threshold, the first selection unit 130 is in a straight traveling state of the target OB. The observation data from the radar apparatus 200 is output to the Kalman filter processing unit 140. For example, when the turning radius R of the target OB derived by the α-β filter processing unit 120 is less than the threshold, the first selection unit 130 determines that the motion state of the target OB is the turning state. The observation data from the radar apparatus 200 is output to the extended Kalman filter processing unit 150.

  Hereinafter, the selection process of the first selection unit 130 and the calculation process of the second selection unit 160 will be described with reference to FIG. FIG. 3 is a timing chart illustrating an example of the selection process performed by the first selection unit 130.

FIG. 3A shows an example of a change in the motion state of the target OB. In FIG. 3A, for example, based on the turning radius R of the target OB derived by the α-β filter processing unit 120, the first selection unit 130 changes the motion state of the target OB from time t _{0 to} t. until ₁ is running straight, until time t ₁ ~t ₃ is turning state, indicating that up to time t ₃ ~t ₅ is determined to be re-straight state. Note that it is assumed that the speed estimation calculation process of the target OB by the estimation arithmetic device 100 has been performed before the time t ₀ .

  FIG. 3B shows the timing at which the Kalman filter processing unit 140 performs Kalman filter processing. FIG. 3C shows the timing at which the extended Kalman filter processing unit 150 performs the extended Kalman filter processing. FIG. 3D shows that the α-β filter processing unit 120 continuously performs the α-β filter processing. As shown in FIGS. 3B to 3D, the α-β filter processing unit 120 always performs filter processing, and only one of the Kalman filter processing unit 140 and the extended Kalman filter processing unit 150 performs filter processing. I do.

FIG. 3E shows that the derivation result output from the second selection unit 160 to the display device 300 as the estimation calculation result of the estimation calculation device 100 is the α-β filter processing unit 120, the Kalman filter processing unit 140, and the extended Kalman filter processing. Indicates which of the sections 150 is. FIG. 3 (e) for example, the time _t 0 ~t ₁ to Kalman filter processing result of the Kalman filtering unit 140, until time _t 1 ~t _2, and until the time _t 3 ~t _{4 α-β} filtering unit The 120 α-β filter processing results indicate that the extended Kalman filter processing result of the extended Kalman filter processing unit 150 is used from time t ₄ to time t ₅ .

Hereinafter, the details of the selection process of the second selection unit 160 will be described. The first selection unit 130 determines that the motion state of the target OB is the straight traveling state based on the α-β filter processing result of the α-β filter processing unit 120 before the time t ₀ . Therefore, the second selection unit 160 derives the estimated position coordinates and the estimated speed of the target OB based on the Kalman filter processing result of the Kalman filter processing unit 140 during the time t _{0 to} t ₁ and outputs them to the display device 300. To do.

The second selection unit 160 derives the estimated position coordinates and the estimated speed of the target OB based on the α-β filter processing result of the α-β filter processing unit 120 at time t ₁ , and displays the display device 300. Output to. At this time, the first selection unit 130 determines the turning radius R, which is the α-β filter processing result derived by the α-β filter processing unit 120, the velocity Vt _{1 of the} object OB, the state vector x (k), the system noise. The vector v (k) is output to the extended Kalman filter processing unit 150. The second selection unit 160 uses the α-β filter processing result of the α-β filter processing unit 120 until a sufficiently high accuracy extended Kalman filter processing result can be acquired from the extended Kalman filter processing unit 150 (time t _{1 to} t ₂ ). Based on this, the estimated position coordinates and estimated speed of the target OB are derived and output to the display device 300.

This is because the extended Kalman filter processing of the extended Kalman filter processing unit 150 requires a certain time until the processing result converges. Therefore, after the extended Kalman filter processing result of the extended Kalman filter processing unit 150 converges (time t _{2 to} t ₃ ), the second selection unit 160 determines the target OB based on the extended Kalman filter processing result of the extended Kalman filter processing unit 150. The estimated position coordinates and the estimated speed are derived and output to the display device 300. Whether or not the extended Kalman filter processing result of the extended Kalman filter processing unit 150 has converged may be determined by, for example, the extended Kalman filter processing unit 150 monitoring the extended Kalman filter processing result to determine whether or not the extended Kalman filter processing unit 150 has converged. It may be determined that the extended Kalman filter processing result has converged when a predetermined filtering convergence period has elapsed from the start of the extended Kalman filter processing of the processing unit 150.

Similarly, the second selection unit 160 derives the estimated position coordinates and the estimated speed of the target OB based on the α-β filter processing result of the α-β filter processing unit 120 at time t ₃ , The data is output to the display device 300. At this time, the first selection unit 130 outputs the state vector x (k) and the system noise vector v (k), which are the α-β filter processing results derived by the α-β filter processing unit 120, to the Kalman filter processing unit 140. . The second selection unit 160 is based on the α-β filter processing result of the α-β filter processing unit 120 until a sufficiently high accuracy Kalman filter processing result can be acquired from the Kalman filter processing unit 140 (time t _{3 to} t ₄ ). Then, the estimated position coordinates and estimated speed of the target OB are derived and output to the display device 300.

The filtering process of the Kalman filter processing unit 140 requires a certain time, like the extended Kalman filter process of the extended Kalman filter processing unit 150. The second selection unit 160, extraction time Kalman filter processing result of the Kalman filtering unit 140 has converged t ₄ later, on the basis of the Kalman filter processing result of the Kalman filtering unit 140, the estimated position coordinates of the target object OB, and the estimated velocity And output to the display device 300. Whether or not the Kalman filter processing result of the Kalman filter processing unit 140 has converged is determined by, for example, the Kalman filter processing unit 140 monitoring the Kalman filter processing result and determining whether or not the Kalman filter processing unit 140 has converged. It may be determined that the Kalman filter processing result has converged when a predetermined filtering convergence period has elapsed since the start of the processing.

  As described above, the estimation calculation device 100 of this embodiment derives the turning radius R geometrically by the α-β filter processing unit 120 from the observation data of at least three points output from the radar device 200, thereby turning the turning radius. R estimation error can be reduced. Further, by performing the switching process of whether the Kalman filter processing unit 140 or the extended Kalman filter processing unit 150 performs the filter processing based on the derived turning radius R by the first selection unit 130 of the present embodiment, Whether the target OB is traveling straight or turning, the accuracy of speed estimation calculation can be improved. In addition, the estimation calculation device 100 of the present embodiment can derive the turning radius R by the α-β filter processing unit 120 from at least three or more observation data, and therefore is required for filter switching accompanying the selection processing by the first selection unit 130. Time can be kept to a minimum.

  According to at least one embodiment described above, the estimated position coordinates, estimated speed, and turning radius R of the target are derived by performing an α-β filter on the observation data of the target acquired from the radar apparatus. α-β filter processing unit 120, Kalman filter processing unit 140 that performs Kalman filter processing on observation data acquired from the radar device to derive estimated position coordinates and an estimated velocity, and observation acquired from the radar device When the extended Kalman filter processing unit 150 that performs the extended Kalman filter processing on the data to derive the estimated position coordinates and the estimated speed of the target and the turning radius R derived by the α-β filter processing unit 120 is larger than the threshold value, The Kalman filter processing unit 140 performs Kalman filter processing, and the α-β filter processing unit 120 derives When the turning radius R is smaller than the threshold value, the first selection unit 130 that causes the extended Kalman filter processing unit 150 to perform the extended Kalman filter processing is provided, thereby reducing the processing load while maintaining the accuracy of speed estimation. .

  Although several embodiments of the present invention have been described, these embodiments are presented by way of example and are not intended to limit the scope of the invention. These embodiments can be implemented in various other forms, and various omissions, replacements, and changes can be made without departing from the spirit of the invention. These embodiments and their modifications are included in the scope and gist of the invention, and are also included in the invention described in the claims and the equivalents thereof.

  DESCRIPTION OF SYMBOLS 100 ... Estimation calculating device, 110 ... Acquisition part, 120 ... (alpha)-(beta) filter process part, 130 ... 1st selection part, 140 ... Kalman filter process part, 150 ... Extended Kalman filter process part, 160 ... 2nd selection part, 200 ... Radar Device, 300 ... display device, OB ... target, R ... turning radius

Claims (4)

An α-β filter processing unit for deriving an estimated position coordinate, estimated speed, and turning radius of the target by performing an α-β filter on the observation data of the target acquired from the radar device;
A Kalman filter processing unit that performs Kalman filter processing on the observation data acquired from the radar device to derive the estimated position coordinates of the target and an estimated velocity;
An extended Kalman filter processing unit for deriving the estimated position coordinates of the target and the estimated velocity by performing extended Kalman filter processing on the observation data acquired from the radar device;
When the turning radius derived by the α-β filter processing unit is larger than a threshold value, the Kalman filter processing unit performs the Kalman filter processing, and when the turning radius derived by the α-β filter processing unit is smaller than the threshold value, A first selection unit that causes the extended Kalman filter processing unit to perform the extended Kalman filter processing;
An estimation arithmetic device comprising: The α-β filter processing is performed during a predetermined filtering convergence period after the Kalman filter processing unit for deriving the estimated position coordinates and the estimated speed by the first selection unit and the extended Kalman filter processing unit are switched. A second selection unit that outputs the estimated position coordinates that are a result of derivation of the unit and the estimated speed;
The estimation arithmetic device according to claim 1. The second selection unit is switched from the Kalman filter processing unit to the extended Kalman filter processing unit until the extended Kalman filter processing of the extended Kalman filter processing unit that started operation converges to the α-β filter processing unit. The estimated position coordinates derived from and the estimated speed are output,
The estimated position coordinates derived by the α-β filter processing unit from when the extended Kalman filter processing unit is switched to the Kalman filter processing unit until the Kalman filter processing of the Kalman filter processing unit that started operation converges, and Outputting the estimated speed,
The estimation arithmetic device according to claim 2. On the computer,
Α-β filter processing is performed on the observation data of the target acquired from the radar device, and the estimated position coordinates, estimated speed, and turning radius of the target are derived.
For the observation data acquired from the radar device, Kalman filter processing is performed to derive the estimated position coordinates of the target, and the estimated speed,
With respect to the observation data acquired from the radar device, the Kalman filter process is performed to derive the estimated position coordinates of the target and the estimated speed,
When the turning radius derived by the α-β filter processing is larger than a threshold value, the Kalman filter processing is performed, and when the turning radius derived by the α-β filter processing is smaller than the threshold value, the extended Kalman filter processing is performed. ,
Estimation calculation program.

Images