JP7272910B2 - In-vehicle device, state estimation method and program - Google Patents

Description

The present invention relates to an in-vehicle device, a state estimation method, and a program.

A car navigation system measures the vehicle speed obtained from a vehicle speed sensor, the angular velocity of the vehicle obtained from a gyro sensor mounted on the vehicle, the current position of the vehicle obtained from a GNSS (Global Navigation Satellite System), and the The current position of the vehicle is estimated based on the direction of travel, and the estimated current position is mapped on a map to inform the driver of the vehicle's position. However, the speed obtained from the vehicle speed sensor and the angular speed obtained from the gyro sensor generally contain an error. Similarly, the current position obtained from GNSS also generally contains errors. In other words, the true position of the vehicle cannot be directly known from the sensor and GNSS output values.

A Kalman filter is known as a technique for solving such problems. A Kalman filter is a method that can estimate the state of a dynamic system using observables with errors. As an example of using a Kalman filter in a car navigation system, for example, the technology described in Patent Document 1 is known.

JP-A-2000-55678

However, observed quantities such as velocity and angular velocity obtained from outputs from the vehicle speed sensor and the gyro sensor often contain errors that are determined to some extent. Kalman filters, on the other hand, are designed on the assumption that the error is white noise. Therefore, the conventional Kalman filter cannot estimate the state of the vehicle with high accuracy.

SUMMARY OF THE INVENTION Accordingly, an object of the present invention is to estimate the state of a vehicle with higher accuracy using a Kalman filter.

An in-vehicle device according to an aspect of the present invention is an in-vehicle device mounted in a vehicle, and includes an acquisition unit that acquires an observable amount related to the state of the vehicle based on an output value acquired from a sensor, and an observable amount that indicates the state of the vehicle. An extended Kalman filter to which a modified Jacobian is applied using a coefficient indicating an error related to the state quantity, and an estimator that calculates the state quantity of the vehicle based on the obtained observed quantity related to the state of the vehicle.

According to the present invention, the vehicle state can be estimated with higher accuracy using the Kalman filter.

It is a figure which shows the hardware structural example of the vehicle-mounted apparatus which concerns on this embodiment. It is a figure which shows the functional block structural example of an in-vehicle apparatus. 4 is a flow chart showing an example of an operation performed by an in-vehicle device;

Embodiments of the present invention will be described with reference to the accompanying drawings. It should be noted that, in each figure, the same reference numerals have the same or similar configurations.

<Device configuration>
FIG. 1 is a diagram showing a hardware configuration example of an in-vehicle device 10 according to this embodiment. The in-vehicle device 10 is mounted on a vehicle, and according to a user's operation, displays a map, displays the current position of the vehicle on the map, searches for a route to a destination, provides route guidance, and the like. In-vehicle device 10 includes processor 11 , storage device 12 , communication IF 13 , input device 14 , output device 15 , GNSS device 16 , and gyro sensor 17 .

The processor 11 is a CPU (Central Processing Unit), a GPU (Graphical Processing Unit), or the like, and controls each part of the in-vehicle device 10 .

The storage device 12 is a memory, a HDD (Hard Disk Drive) and/or an SSD (Solid State Drive), etc., and stores various data necessary for the in-vehicle device 10 to operate.

The communication IF 13 is an interface for connecting with a communication device such as a mobile phone or a smart phone.

The input device 14 is a device that receives various operations from the user, and is a switch, button, touch panel, microphone, or the like provided in the in-vehicle device 10 . The input device 14 also receives input of vehicle speed data output from a vehicle speed sensor 18 attached to the vehicle on which the in-vehicle device 10 is mounted.

The output device 15 is, for example, a display and/or a speaker.

The GNSS device 16 measures the position of the in-vehicle device 10 and the direction of travel of the vehicle (hereinafter referred to as "vehicle direction") based on GNSS signals transmitted from GNSS satellites. GNSS also includes GPS.

The gyro sensor 17 is, for example, a vibration gyro sensor, and detects the rotational speed of the vehicle, that is, the angular velocity of the vehicle.

<Functional block configuration>
FIG. 2 is a diagram showing a functional block configuration example of the in-vehicle device 10. As shown in FIG. In-vehicle device 10 includes storage unit 101 , acquisition unit 102 , and estimation unit 103 . The storage unit 101 can be implemented using the storage device 12 included in the in-vehicle device 10 . Acquisition unit 102 and estimation unit 103 can be implemented by processor 11 of in-vehicle device 10 executing a program stored in storage device 12 . Also, the program can be stored in a storage medium. The storage medium storing the program may be a non-transitory computer readable medium. The non-temporary storage medium is not particularly limited, but may be a storage medium such as a USB memory or CD-ROM, for example.

The storage unit 101 stores map data 101a.

Acquisition unit 102 acquires an observable quantity regarding the state of the vehicle based on the output value acquired from the sensor. The observables regarding the state of the vehicle include the velocity in the traveling direction of the vehicle (hereinafter referred to as "vehicle velocity"), the angular velocity of the vehicle, the position of the vehicle, and the traveling direction of the vehicle. More specifically, the acquisition unit 102 acquires the position of the vehicle and the traveling direction of the vehicle observed by the GNSS device 16, acquires the angular velocity of the vehicle observed by the gyro sensor 17, and acquires the angular velocity of the vehicle observed by the vehicle speed sensor 18. Get the observed vehicle speed.

Based on an extended Kalman filter to which a modified Jacobian (hereinafter referred to as a "corrected Jacobian") is applied using a coefficient indicating an error related to the state quantity indicating the state of the vehicle, and an observed quantity regarding the state of the vehicle, the estimating unit 103 Then, a state quantity indicating the state of the vehicle (hereinafter referred to as "vehicle state quantity") is calculated (estimated). The vehicle state quantities include vehicle speed, vehicle angular velocity, vehicle position, and vehicle traveling direction. The modified Jacobian will be described later.

<Estimation of State Quantity of Vehicle Using Extended Kalman Filter>
First, the state quantity of the vehicle estimated by the extended Kalman filter is defined as follows.

式（１）において、添え字のｔは時刻を示す。例えば、式（１）の左辺は、時刻ｔにおける車両の状態量を示しており、右辺は、時刻ｔ－１における車両の状態量を示している。ｄｔは、所定の単位時間を示しており、車載装置１０が、拡張カルマンフィルタを用いて車両の状態量を推定する周期の間隔に相当する。当該間隔は、例えば、取得部１０２が、車両の状態に関する観測量を取得する間隔であってもよい。右辺の第２項であるｑ_tは、平均が０であり誤差共分散行列Ｑ_tである正規分布Ｎ（０、Ｑ_t）に従うシステム雑音である。 v: Velocity of vehicle ω: Angular velocity of vehicle x: x-coordinate of vehicle position y: y-coordinate of vehicle position θ: orientation of vehicle Here, the state vector representing the state quantity of the vehicle is expressed as (v, ω , x, y, .theta.), the state equation for the state quantity of the vehicle can be represented by equation (1).

In Equation (1), the subscript t indicates time. For example, the left side of equation (1) indicates the state quantity of the vehicle at time t, and the right side indicates the state quantity of the vehicle at time t−1. dt indicates a predetermined unit time, and corresponds to a period interval at which the in-vehicle device 10 estimates the state quantity of the vehicle using the extended Kalman filter. The interval may be, for example, an interval at which the acquisition unit 102 acquires an observable amount regarding the state of the vehicle. The second term on the right hand side, q _t , is system noise following a normal distribution N(0, Q _t ) with zero mean and error covariance matrix Q _t .

According to the state equation shown in formula (1), the x-coordinate of the vehicle position at time t is obtained by adding the x-axis component of the vehicle velocity multiplied by unit time to the vehicle position at time t-1. It shows that it is calculated by Also, the y-coordinate of the vehicle position at time t is calculated by adding a value obtained by multiplying the y-axis direction component of the vehicle velocity by unit time to the vehicle position at time t-1. . Also, the azimuth of the vehicle at time t is calculated by adding an angle obtained by multiplying the angular velocity of the vehicle by the unit time to the azimuth of the vehicle at time t−1.

Next, the observable amount relating to the state of the vehicle acquired by the acquisition unit 102 is defined as follows.

v _pls : Velocity of the vehicle obtained from the output (pulse data, etc.) of the vehicle speed sensor 18 ω _gyro : Angular velocity of the vehicle observed from the output of the gyro sensor 17 x _gps : Position of the vehicle observed from the positioning result of the GNSS device 16 y _gps : y coordinate of the position of the vehicle observed from the positioning result of the GNSS device 16 θ _gps : azimuth of the vehicle observed from the positioning result of the GNSS device 16 Here, observation in which the above observation amount is expressed as a vector Assuming that the vector is (v _pls , ω _gyro , x _gps , y _gps , θ _gps ), the observation equation for the observed quantity can be expressed by Equation (2). The second term on the right side, r _t , is the observed noise following a normal distribution N(0, R _t ) with mean 0 and error covariance matrix R _t .

Next, the processing performed by the extended Kalman filter will be described by dividing it into "prediction processing" for predicting the state quantity of the vehicle and "estimation processing" for estimating the state quantity of the vehicle. The extended Kalman filter repeatedly estimates the state quantity of the vehicle with a period of dt. The prediction process is to predict the state quantity of the vehicle in the current cycle using the state quantity of the vehicle estimated in the previous cycle and the state equation. In addition, the estimation process is to estimate the state quantity of the vehicle with higher accuracy by correcting the predicted state quantity of the vehicle in the current cycle with the observed quantity related to the state of the vehicle obtained in the current cycle. is.

In the following description, a value with a suffix t|t-1 indicates a predicted value (current predicted value) at time t calculated based on information up to time t-1. A value with the subscript t|t indicates an estimated value at time t (current estimated value) calculated based on information up to time t. Also, a value with the subscript t-1|t-1 indicates an estimated value (previous estimated value) at time t-1 estimated based on information up to time t-1.

式（３）の左辺は、時刻ｔ－１までの情報に基づき予測された時刻ｔにおける車両状態予測値を示す。例えば、車両の位置のｘ座標の予測値を示すｘ_t|t-1は、時刻ｔ－１までの情報に基づき推定された時刻ｔ－１における、車両の位置のｘ座標（ｘ_t-1|t-1）の推定値と、車両の方位（θ_t-1|t-1）の推定値と、車両の速度（ｖ_t-1|t-1）の推定値とに基づき算出される。 (prediction processing)
More specifically, the prediction process in the extended Kalman filter is a process of calculating a predicted value of the state quantity of the vehicle (hereinafter referred to as "predicted vehicle state value") and a predicted value of the error covariance matrix. A formula for calculating the vehicle state prediction value is shown in formula (3).

The left side of equation (3) indicates the vehicle state prediction value at time t predicted based on the information up to time t-1. For example, x _t|t-1 indicating the predicted value of the x-coordinate of the vehicle position is the x-coordinate of the vehicle position (x _{t-1 |t-1} ), the estimated vehicle heading (θ _t-1|t-1 ), and the estimated vehicle velocity (v _t-1|t-1 ) .

式（４）の左辺に示す誤差共分散行列は、時刻ｔ－１までの情報に基づき推定された時刻ｔ－１における誤差共分散行列に基づき算出される。なお、Ｆ_t-1は、式（１）の状態方程式から求められる、時刻ｔ－１におけるヤコビアン（ヤコビ行列とも言う）を示す。また、Ｆにおける添え字のＴは、転置行列を示す。また、Ｑは、時刻ｔ－１におけるシステム雑音の誤差共分散行列を示す。 Equation (4) shows the predicted value of the error covariance matrix, that is, the equation for calculating the error covariance matrix P _t|t−1 at time t predicted based on the information up to time t−1. Note that the variance is the average of the squared values of the error. That is, the variance of the state quantity of the vehicle is the average of the squared errors of the state quantity of the vehicle. Therefore, calculating the predicted value of the error covariance matrix corresponds to calculating the error of the vehicle state predicted value.

The error covariance matrix shown on the left side of Equation (4) is calculated based on the error covariance matrix at time t−1 estimated based on the information up to time t−1. Note that F _t-1 indicates the Jacobian (also called Jacobian matrix) at time t-1, which is obtained from the state equation of equation (1). Also, the subscript T in F indicates a transposed matrix. Also, Q represents the error covariance matrix of the system noise at time t−1.

Thus, in the prediction process, the vehicle state prediction value at time t calculated based on the information up to time t−1 and the prediction of the error covariance matrix at time t calculated based on the information up to time t−1 Calculate the value and

(estimation process)
Estimation processing in the extended Kalman filter is based on the vehicle state prediction value calculated in the prediction processing and the prediction value of the error covariance matrix, the estimated value of the state quantity of the vehicle (hereinafter referred to as "vehicle state estimate value") ) and an estimated value of the error covariance matrix.

式（５）において、左辺は、観測残差をベクトル表記した観測残差ベクトルである。また、式（５）において、右辺の第１項は、時刻ｔにおける観測対象ベクトルである。また、式（５）において、右辺の第２項は、予測処理で予測した車両状態予測値である。つまり、観測残差とは、車両の状態に関する観測量と、車両状態予測値から算出される車両状態推定値との差分である。 First, the formula for calculating the observation residual is shown in formula (5).

In Equation (5), the left side is an observed residual vector representing the observed residual as a vector. Also, in Equation (5), the first term on the right side is the observed vector at time t. In Equation (5), the second term on the right side is the vehicle state prediction value predicted by the prediction process. That is, the observation residual is the difference between the observed amount of the vehicle state and the vehicle state estimated value calculated from the vehicle state predicted value.

式（６）の左辺は、時刻ｔまでの情報に基づき推定された時刻ｔにおける車両状態推定値を示す。車両状態推定値は、式（６）の右辺に示すように、時刻ｔ－１までの情報に基づき推定された時刻ｔにおける車両状態予測値を、観測残差で補正することで算出される。ここで、車両状態予測値を観測残差にて補正する際の補正係数としてＫ_tが用いられる。Ｋ_tは、カルマンゲインと呼ばれ、式（７）で表される。

式（７）において、Ｒ_tは、観測雑音の誤差共分散行列である。また、添え字の「－１
」は、逆行列を示す。式（７）に示すカルマンゲインＫ_tは、時刻ｔ－１までの情報に基づく時刻ｔにおける誤差共分散行列の予測値Ｐ_t|t-1に基づき算出される。 Next, the vehicle state estimation value is calculated by Equation (6) using the observation residual shown in Equation (5).

The left side of Equation (6) indicates the vehicle state estimated value at time t estimated based on the information up to time t. The vehicle state estimation value is calculated by correcting the vehicle state prediction value at time t estimated based on the information up to time t−1 with the observation residual, as shown on the right side of equation (6). Here, K _t is used as a correction coefficient when correcting the vehicle state prediction value with the observation residual. K _t is called the Kalman gain and is represented by Equation (7).

In equation (7), R _t is the error covariance matrix of the observation noise. Also, the subscript "-1
” indicates the inverse matrix. The Kalman gain K _t shown in Equation (7) is calculated based on the predicted value P _t|t-1 of the error covariance matrix at time t based on the information up to time t-1.

In equation (6), the Kalman gain K _t is a coefficient that determines whether the estimated vehicle state value is to be emphasized or the observed quantity of the vehicle state is to be emphasized when calculating the vehicle state estimated value. For example, if the errors in the observables obtained from the GNSS device 16, the gyro sensor 17, and the vehicle speed sensor 18 are sufficiently small, that is, if the observables regarding the state of the vehicle are reliable values, the estimated vehicle state is close to the observables. value is desirable.

On the other hand, if the error in the vehicle state prediction value is sufficiently smaller than the observed quantity based on the outputs from the GNSS device 16, the gyro sensor 17, and the vehicle speed sensor 18, that is, if the observed quantity regarding the vehicle state is not very reliable, the vehicle state It is desirable that the estimated value should be close to the vehicle state prediction value. For example, if the Kalman gain K _k =0 is given to equation (6), the vehicle state estimated value becomes equal to the vehicle state predicted value. Thus, the Kalman gain _Kt is a coefficient that is set so that the vehicle state estimation value is an appropriate value.

式（８）において、Ｉは、単位行列を示す。Ｋ_tは、式（７）で算出された、時刻ｔにおけるカルマンゲインである。Ｐ_t|t-1は、時刻ｔ－１までの情報に基づき予測された時刻ｔにおける誤差共分散行列（つまり、式（４）で算出された誤差共分散行列）である。 Next, the estimated value of the error covariance matrix, that is, the error covariance matrix Pt|t at time t estimated based on the information up to time t is calculated by Equation (8).

In Equation (8), I indicates a unit matrix. K _t is the Kalman gain at time t calculated by Equation (7). P _t|t−1 is the error covariance matrix at time t predicted based on the information up to time t−1 (that is, the error covariance matrix calculated by Equation (4)).

The processing performed by the estimation unit 103 based on the processing procedure of the extended Kalman filter described above can be described as follows.

(1) Calculate the predicted value of the state quantity of the vehicle based on the previous state quantity of the vehicle and the state equation of the vehicle, and calculate the predicted value of the error covariance matrix based on the previous error covariance matrix and the Jacobian. (2) Calculate the observation residual, which is the difference between the observed quantity and the predicted value of the state quantity (3) Calculate the Kalman gain based on the predicted value of the error covariance matrix (4) The previous state quantity of the vehicle and the observed residual Calculate the state quantity of the vehicle based on the difference and the Kalman gain. (5) Calculate the error covariance matrix based on the previous error covariance matrix and the Kalman gain.

In the Kalman filter, the accuracy of the estimated vehicle state quantity depends on the Kalman gain _Kt . If the Kalman gain K _t is not correct, for example, even though the observed quantity of the vehicle state is unreliable, the estimated vehicle state value may be corrected to be closer to the observed quantity.

The extended Kalman filter assumes that system noise and observation noise are white noise. In other words, the extended Kalman filter is based on the premise that the mean error is zero. However, in the real world, the errors included in the values obtained from the gyro sensor 17 and the vehicle speed sensor 18 are not white noise with an average of 0, but errors that are always offset. In other words, when the values obtained from the gyro sensor 17 and the vehicle speed sensor 18 are averaged, the average error often accumulates gradually rather than being zero.

If the extended Kalman filter is used for observations with such errors, the error covariance matrix cannot be calculated properly. As a result, the Kalman gain _Kt is also not calculated accurately, so there is a problem that the state quantity of the vehicle cannot be accurately estimated.

Therefore, in the present embodiment, a modified Jacobian obtained by correcting the Jacobian corresponding to the state equation using a coefficient indicating an error related to the state quantity of the vehicle is used to calculate the state quantity of the vehicle with higher accuracy.

式（９）に示す行列のうち、１行１列目から１行５列目までは、式（１）に示す状態法定式の右辺の縦ベクトルにおける行目を、それぞれｖ、ω、ｘ、ｙ及びθで微分した値である。２行１列目から２行５列目までは、式（１）に示す状態法定式の右辺の縦ベクトルにおける２行目を、それぞれｖ、ω、ｘ、ｙ及びθで微分した値である。３行１列目から３行５列目までは、式（１）に示す状態法定式の右辺の縦ベクトルにおける３行目を、それぞれｖ、ω、ｘ、ｙ及びθで微分した値である。４行１列目から４行５列目までは、式（１）に示す状態法定式の右辺の縦ベクトルにおける４行目を、それぞれｖ、ω、ｘ、ｙ及びθで微分した値である。５行１列目から５行５列目までは、式（１）に示す状態法定式の右辺の縦ベクトルにおける５行目を、それぞれｖ、ω、ｘ、ｙ及びθで微分した値である。 First, Equation (9) shows the Jacobian corresponding to the state law formulation of Equation (1), which is calculated based on the concept of the conventional Kalman filter. In the following description, the Jacobian shown in Equation (9) may be referred to as the "conventional Jacobian".

In the matrix shown in Equation (9), the first row, first column to the first row, fifth column are v, ω, x, and the row of the vertical vector on the right side of the state law formula shown in Equation (1). It is a value differentiated with respect to y and θ. The 2nd row, 1st column to the 2nd row, 5th column are the values obtained by differentiating the 2nd row of the vertical vector on the right side of the state law formula shown in Equation (1) with v, ω, x, y and θ, respectively. . The 3rd row, 1st column to the 3rd row, 5th column are values obtained by differentiating the 3rd row in the vertical vector on the right side of the state law formula shown in Equation (1) with v, ω, x, y and θ, respectively. . The 4th row, 1st column to the 4th row, 5th column are the values obtained by differentiating the 4th row of the vertical vector on the right side of the state law formula shown in Equation (1) with v, ω, x, y, and θ, respectively. . The 5th row, 1st column to the 5th row, 5th column are values obtained by differentiating the 5th row in the vertical vector on the right side of the state law formula shown in Equation (1) with v, ω, x, y, and θ, respectively. .

また、ｄｘ、ｄｙ及びｄθの値を、それぞれ、式（１１）、式（１２）及び式（１３）に示す。

式（１１）に示す係数ｄｘ（第１係数）は、車両の位置のうちｘ軸（第１軸）方向における車両の位置誤差を示す。式（１２）に示す係数ｄｙ（第２係数）は、車両の位置のうちｙ軸（第１軸）方向における車両の位置誤差を示す。式（１３）に示す係数ｄθ（第３係数）は、車両の位置のうち進行方向における車両の位置誤差を示す。 Next, the modified Jacobian used in this embodiment is shown in Equation (10).

Also, the values of dx, dy and dθ are shown in equations (11), (12) and (13), respectively.

The coefficient dx (first coefficient) shown in Equation (11) indicates the vehicle position error in the x-axis (first axis) direction of the vehicle position. The coefficient dy (second coefficient) shown in Equation (12) indicates the vehicle position error in the y-axis (first axis) direction of the vehicle position. The coefficient dθ (third coefficient) shown in Equation (13) indicates the position error of the vehicle in the traveling direction of the vehicle position.

More specifically, the coefficient (first coefficient) shown in equation (11) is the standard deviation corresponding to the variance of the error in the direction of the first axis in the previous estimated value of the error covariance matrix and the vehicle speed and the standard deviation corresponding to the variance of the error of .

The coefficient (second coefficient) shown in Equation (12) corresponds to the standard deviation corresponding to the variance of the error in the direction of the second axis and the variance of the vehicle speed error in the traveling direction of the previous estimated value of the error covariance matrix. It is calculated based on the standard deviation of

The coefficient (third coefficient) shown in Equation (13) is the standard deviation corresponding to the variance of the error in the traveling direction of the vehicle and the standard deviation corresponding to the variance of the error in the angular velocity of the vehicle in the previous estimate of the error covariance matrix. calculated based on

式（１４）に示すように、誤差共分散行列の対角成分は、車両の位置、車両の方位、車両の速度、及び、車両の角速度に関する各々の分散である。また、対角成分以外の成分は、車両の位置、車両の方位、車両の速度、及び、車両の角速度のうち２つの組み合わせに関する共分散である。なお、分散は、誤差の散らばり具合を示しており、共分散は、２種類の車両の状態量における相関関係を示している。 Here, an example of each component of the error covariance matrix is shown in Equation (14).

As shown in equation (14), the diagonal elements of the error covariance matrix are the respective variances for vehicle position, vehicle heading, vehicle velocity, and vehicle angular velocity. Components other than the diagonal component are covariances relating to a combination of two of vehicle position, vehicle orientation, vehicle velocity, and vehicle angular velocity. The variance indicates how the errors are scattered, and the covariance indicates the correlation between the state quantities of the two types of vehicles.

Therefore, when calculating the error covariance matrix using Equation (4), estimating section 103 replaces Jacobian F _t-1 with the modified Jacobian shown in Equation (10) to calculate the Kalman gain. At this time, the estimation unit 103 calculates the standard deviation of the v error, the standard deviation of the ω error, the standard deviation of the x error, the standard deviation of the y error, and the standard deviation of the θ error using the formula ( 4) In the error covariance matrix P _{t-1 | t-1,} the 1st row and 1st column components, the 2nd row and 2nd column components, the 3rd row and 3rd column components, the 4th row and 4th column components, and It is obtained from the 5th row, 5th column component and substituted into the equations (11) to (13).

By setting the coefficients indicating the error related to the state quantity of the vehicle to the components of the 3rd row, 3rd column, 4th row, 4th column, and 5th row, 5th column of the modified Jacobian F _{t |t-1} , the formula ( 9), in the error covariance matrix Pt _|t-1 calculated by Equation (4), the x-coordinate variance, y-coordinate variance, and θ variance of the vehicle position are The corresponding values of the 3rd row, 3rd column component, the 4th row, 4th column component, and the 5th row, 5th column diagonal component are increased. In addition, in Equation (7), by calculating the Kalman gain K _t using the error covariance matrix P _{t|t-1 in which} the value of the diagonal component is increased, the 3rd row, 3rd column in the Kalman gain K _t The values of the diagonal components of the fourth row, fourth column, and fifth row, fifth column also increase. Then, in Equation (6), when estimating the state quantity of the vehicle at time t using the Kalman filter according to the present embodiment, the conventional Kalman filter that assumes that white noise with an average of 0 is included is Compared to the case of using GNSS, the x-coordinate, y-coordinate, and θ of the vehicle state quantities reflect more of the observed quantity obtained from the GNSS than the vehicle state prediction value.

As a result, when estimating the state of the vehicle using the gyro sensor 17 and the vehicle speed sensor 18, which always have offset errors instead of white noise, the x-coordinate, y-coordinate, and θ are obtained from the GNSS. Since many corrections are made based on the acquired vehicle position (x, y, θ), the vehicle position can be estimated with higher accuracy.

<Operations performed by the in-vehicle device>
FIG. 3 is a flowchart showing an example of the operation performed by the in-vehicle device 10. As shown in FIG. First, the acquisition unit 102 acquires observable quantities related to vehicle speed, vehicle angular velocity, vehicle position, and vehicle orientation from the GNSS device 16, the gyro sensor 17, and the vehicle speed sensor 18 (S10). Subsequently, the estimating unit 103 uses the Kalman filter replaced with the Jacobian shown in Equations (10) to (13) to determine the vehicle speed, vehicle angular velocity, vehicle position, and vehicle orientation. Estimate (S11). Subsequently, the display control unit 104 updates the vehicle position and vehicle orientation drawn on the map based on the vehicle position and vehicle orientation estimated by the estimation unit 103 (S12). When ending the updating of the vehicle position (for example, when ending the display of the navigation screen), the process ends. When continuing to update the vehicle position, the process returns to step S10.

<Summary>
According to the embodiment described above, by using the Jacobian including the coefficients indicating the error related to the state quantity of the vehicle, even when using a sensor having an error such that the average does not become 0 instead of white noise, , it becomes possible to estimate the position of the vehicle with higher accuracy.

The embodiments described above are for facilitating understanding of the present invention, and are not intended to limit and interpret the present invention. Flowcharts, sequences, elements included in the embodiments, their arrangement, materials, conditions, shapes, sizes, and the like described in the embodiments are not limited to those illustrated and can be changed as appropriate. Also, it is possible to partially replace or combine the configurations shown in different embodiments.

DESCRIPTION OF SYMBOLS 10... Vehicle-mounted apparatus, 11... Processor, 12... Storage device, 13... Communication IF, 14... Input device, 15... Output device, 16... GNSS apparatus, 17... Gyro sensor, 18... Vehicle speed sensor, 101... Storage part, 101a ... map data, 102 ... acquisition unit, 103 ... estimation unit, 104 ... display control unit

Claims (6)

An in-vehicle device mounted in a vehicle,
an acquisition unit that acquires an observable amount related to the state of the vehicle based on the output value acquired from the sensor;
Based on an extended Kalman filter to which a Jacobian whose diagonal component is corrected using a coefficient indicating an error related to the state quantity indicating the state of the vehicle is applied, and the obtained observed quantity regarding the state of the vehicle, the state quantity of the vehicle is calculated. an estimation unit that calculates;
In-vehicle device having The state quantity of the vehicle includes the position of the vehicle, the traveling direction of the vehicle, and the vehicle speed in the traveling direction of the vehicle,
the coefficients include a first coefficient related to a vehicle position error in a first axis, a second coefficient related to a vehicle position error in a second axis, and a third coefficient related to a heading error of the vehicle;
The in-vehicle device according to claim 1 . The estimation unit
calculating a predicted value of the state quantity of the vehicle based on the previous state quantity of the vehicle and the state equation of the vehicle;
Calculate the predicted value of the error covariance matrix based on the previous error covariance matrix and the Jacobian,
calculating an observation residual that is the difference between the observed quantity and the predicted value of the state quantity;
Calculate the Kalman gain based on the predicted value of the error covariance matrix,
calculating a state quantity of the vehicle based on the previous state quantity of the vehicle, the observed residual, and the Kalman gain;
Calculate an error covariance matrix based on the previous error covariance matrix and the Kalman gain;
The in-vehicle device according to claim 2. The first coefficient is calculated based on the standard deviation corresponding to the variance of the errors in the first axis direction and the standard deviation corresponding to the variance of the vehicle speed errors in the traveling direction of the vehicle in the previous error covariance matrix. is,
The second coefficient is calculated based on the standard deviation corresponding to the variance of the error in the second axis direction and the standard deviation corresponding to the variance of the vehicle speed error in the traveling direction of the vehicle in the previous error covariance matrix. is,
The third coefficient is calculated based on the standard deviation corresponding to the variance of the error in the traveling direction of the vehicle and the standard deviation corresponding to the variance of the angular velocity error of the vehicle in the previous error covariance matrix.
The in-vehicle device according to claim 3. A state estimation method executed by an in-vehicle device mounted in a vehicle,
obtaining observables regarding the state of the vehicle based on output values obtained from sensors;
Based on an extended Kalman filter to which a Jacobian whose diagonal component is corrected using a coefficient indicating an error related to the state quantity indicating the state of the vehicle is applied, and the obtained observed quantity regarding the state of the vehicle, the state quantity of the vehicle is calculated. a calculating step;
State estimation methods, including computer installed in the vehicle,
obtaining observables regarding the state of the vehicle based on output values obtained from sensors;
Based on an extended Kalman filter to which a Jacobian whose diagonal component is corrected using a coefficient indicating an error related to the state quantity indicating the state of the vehicle is applied, and the obtained observed quantity regarding the state of the vehicle, the state quantity of the vehicle is calculated. a calculating step;
program to run the

Images