WO2017150106A1 - 車載装置、及び、推定方法 - Google Patents

車載装置、及び、推定方法 Download PDF

Info

Publication number
WO2017150106A1
WO2017150106A1 PCT/JP2017/004510 JP2017004510W WO2017150106A1 WO 2017150106 A1 WO2017150106 A1 WO 2017150106A1 JP 2017004510 W JP2017004510 W JP 2017004510W WO 2017150106 A1 WO2017150106 A1 WO 2017150106A1
Authority
WO
WIPO (PCT)
Prior art keywords
vehicle
error
value
state
calculated
Prior art date
Application number
PCT/JP2017/004510
Other languages
English (en)
French (fr)
Inventor
和弘 岡田
Original Assignee
クラリオン株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by クラリオン株式会社 filed Critical クラリオン株式会社
Priority to CN201780014609.6A priority Critical patent/CN108700423B/zh
Priority to US16/072,352 priority patent/US11036231B2/en
Priority to EP17759577.4A priority patent/EP3425338A4/en
Publication of WO2017150106A1 publication Critical patent/WO2017150106A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar
    • G01S13/723—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar by using numerical data
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H17/00—Networks using digital techniques
    • H03H17/02—Frequency selective networks
    • H03H17/0248—Filters characterised by a particular frequency response or filtering method
    • H03H17/0255—Filters based on statistics
    • H03H17/0257—KALMAN filters
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS

Definitions

  • the present invention relates to an in-vehicle device and an estimation method.
  • Patent Document 1 JP-A-2000-55678.
  • Patent Document 1 “based on the movement distance and the azimuth change amount calculated by the movement distance calculation unit 11 and the azimuth change amount calculation unit 12 based on the detection values of the vehicle speed sensor 4 and the gyro 6, the relative trajectory calculation unit 13, A Kalman filter (error estimation) using a difference between estimated navigation data (vehicle speed, absolute bearing, absolute position) calculated by the absolute position calculator 14 and GPS positioning data (speed, bearing, position) from the GPS receiver 8 as an observation value.
  • estimated navigation data vehicle speed, absolute bearing, absolute position
  • GPS positioning data speed, bearing, position
  • an error may accumulate in the moving distance and the azimuth change amount obtained from the observation amount such as the speed and the angular velocity observed based on the output from the sensor such as the vehicle speed pulse and the gyro.
  • Patent Document 1 there is no disclosure about a technique for estimating the state of a vehicle by a Kalman filter in consideration of accumulated errors, and the state of the vehicle cannot be accurately estimated by a Kalman filter. Therefore, an object of the present invention is to make it possible to accurately estimate the state of a vehicle using a Kalman filter.
  • an in-vehicle device is an in-vehicle device mounted on a vehicle, and based on an output from a sensor, an observation unit that observes an observation amount related to the variation of the vehicle, and a Kalman filter, A control unit that estimates a state quantity indicating a state, wherein the control unit calculates a predicted value of the state quantity of the vehicle, and the observation quantity is an error of the state quantity that is in a calculus relationship with the observation quantity.
  • An error of the predicted value is calculated by the Kalman filter to which an error of a quantity is input, and the estimated value of the state quantity of the vehicle is calculated by the Kalman filter based on the calculated predicted value and the error of the predicted value.
  • An error of the estimated value is calculated.
  • the state of the vehicle can be accurately estimated by the Kalman filter.
  • FIG. 1 is a block diagram showing the configuration of the navigation device.
  • FIG. 2 is a flowchart showing the operation of the navigation device.
  • FIG. 3 is a flowchart showing the operation of the estimation unit in the vehicle state estimation process.
  • FIG. 4 is a diagram for explaining map matching processing based on the estimated vehicle state.
  • FIG. 1 is a block diagram showing the configuration of the navigation device 1 (on-vehicle device).
  • the navigation device 1 is an in-vehicle device mounted on a vehicle, and displays a map, displays the current position of the vehicle on the map, searches for a route to a destination, route guidance, and the like according to an operation of a user on the vehicle. Execute. Note that the navigation device 1 may be fixed to a dashboard of the vehicle or the like, or may be detachable from the vehicle.
  • the navigation device 1 includes a control unit 2, a storage unit 3, a touch panel 4, a GPS reception unit 5, a vehicle speed sensor 6 (sensor), and a gyro sensor 7 (angular velocity sensor, sensor). And an acceleration sensor 8 (sensor).
  • the control unit 2 includes a CPU, a ROM, a RAM, other control circuits, and the like, and controls each unit of the navigation device 1.
  • the control unit 2 functions as an observation unit 21 and an estimation unit 22 to be described later by executing a control program stored in the ROM, the storage unit 3 or the like.
  • the storage unit 3 includes a hard disk, a nonvolatile memory such as an EEPROM, and stores data in a rewritable manner.
  • the storage unit 3 stores map data 3a in addition to the control program executed by the control unit 2.
  • the map data 3a includes node information relating to nodes indicating intersections and other connection points on the road network, link information relating to links indicating road sections between nodes, information relating to map display, and the like.
  • the link information includes at least information regarding the position of the link and information regarding the direction of the link for each link.
  • the touch panel 4 includes a display panel 4a and a touch sensor 4b.
  • the display panel 4 a is configured by a liquid crystal display, an EL (Electro Luminescent) display, or the like, and displays various information on the display panel 4 a under the control of the control unit 2.
  • the touch sensor 4b is arranged so as to overlap the display panel 4a, detects a user's touch operation, and outputs a signal indicating a position where the touch operation is performed to the control unit 2.
  • the controller 2 executes processing corresponding to the touch operation based on the input from the touch sensor 4b.
  • the GPS receiving unit 5 receives GPS radio waves from GPS satellites via the GPS antenna 5a, and from the GPS signals superimposed on the GPS radio waves, the position of the vehicle and the direction of the traveling direction of the vehicle (hereinafter referred to as “vehicle direction”). Is expressed by calculation.
  • the GPS receiving unit 5 outputs information indicating the position of the vehicle and information indicating the direction of the vehicle to the control unit 2.
  • the vehicle speed sensor 6 detects the vehicle speed of the vehicle and outputs a signal indicating the detected vehicle speed to the control unit 2.
  • the gyro sensor 7 is composed of, for example, a vibration gyro and detects an angular velocity due to the rotation of the vehicle.
  • the gyro sensor 7 outputs a signal indicating the detected angular velocity to the control unit 2.
  • the acceleration sensor 8 detects the acceleration (for example, the inclination of the vehicle with respect to the traveling direction) acting on the vehicle.
  • the acceleration sensor 8 outputs a signal indicating the detected acceleration to the control unit 2.
  • control unit 2 includes an observation unit 21 and an estimation unit 22.
  • the observation unit 21 observes an observation amount relating to vehicle fluctuations based on signals output from the vehicle speed sensor 6, the gyro sensor 7, and the acceleration sensor 8.
  • the observation unit 21 observes the vehicle speed (observation amount) as an observation amount based on a signal indicating the vehicle speed output from the vehicle speed sensor 6.
  • the observation unit 21 calculates a vehicle speed and a vehicle speed error from a signal indicating the vehicle speed by a predetermined calculation.
  • the calculated vehicle speed error is an error accumulated in the travel distance obtained from this speed.
  • the observation unit 21 observes an angular velocity (observation amount) due to the rotation of the vehicle as an observation amount based on a signal indicating the angular velocity output from the gyro sensor 7.
  • the observation unit 21 calculates an angular velocity of the vehicle and an error in the angular velocity of the vehicle from a signal indicating the angular velocity by a predetermined calculation.
  • the calculated error of the angular velocity of the vehicle is an error accumulated in the azimuth change amount obtained from the angular velocity.
  • the observation unit 21 observes the acceleration of the vehicle as an observation amount based on the signal indicating the acceleration (observation amount) output from the acceleration sensor 8.
  • the observation unit 21 calculates a vehicle acceleration and a vehicle acceleration error from a signal indicating the acceleration by a predetermined calculation.
  • the calculated vehicle acceleration error is an error accumulated in the speed obtained from the acceleration.
  • observation unit 21 observes the position of the vehicle and the direction of the vehicle based on the information indicating the position of the vehicle output from the GPS receiving unit 5 and the information indicating the direction of the vehicle.
  • the estimation unit 22 estimates a state quantity indicating the state of the vehicle by a Kalman filter based on the vehicle speed, the vehicle acceleration, the vehicle angular velocity, the vehicle position, and the vehicle direction observed by the observation unit 21. In the present embodiment, the estimation unit 22 estimates the position of the vehicle, the direction of the vehicle, the speed of the vehicle, and the angular velocity of the vehicle as the state quantity of the vehicle.
  • control unit 2 evaluates a link to be subjected to map matching based on the vehicle state quantity estimated by the estimation unit 22.
  • the vehicle speed estimated by the estimation unit 22 corresponds to the state quantity of the vehicle. Further, the speed of the vehicle observed by the observation unit 21 corresponds to the observation amount. Similarly, the angular velocity of the vehicle estimated by the estimation unit 22 corresponds to the state quantity of the vehicle. Moreover, the angular velocity of the vehicle observed by the observation unit 21 corresponds to the observation amount.
  • the vehicle state quantities estimated by the Kalman filter are the vehicle position, the vehicle orientation, the vehicle speed, and the angular velocity of the vehicle. Below, the state quantities of the vehicle to be estimated are shown.
  • x x coordinate of the vehicle position y: y coordinate of the vehicle position ⁇ : vehicle direction v: vehicle speed ⁇ : angular velocity of the vehicle
  • the subscript k indicates time. For example, (x k + 1 , y k + 1 , ⁇ k + 1 , v k + 1 , ⁇ k + 1) on the left side of the equation (1) indicates the state quantity of the vehicle at time k + 1.
  • a second term in the right side q k is the system noise (mean 0, normal distribution N with Q k is the error covariance matrix (0, Q k)).
  • the error covariance matrix is a matrix of variance and covariance.
  • the observation unit 21 observes the vehicle speed, the vehicle angular velocity, the vehicle acceleration, the vehicle position, and the vehicle orientation as observation targets. As described above, the observation unit 21 observes the vehicle speed based on the output from the vehicle speed sensor 6. The observation unit 21 observes the angular velocity of the vehicle based on the output from the gyro sensor 7. The observation unit 21 observes the acceleration of the vehicle based on the output from the acceleration sensor 8. The observation unit 21 observes the position of the vehicle and the direction of the vehicle based on the output from the GPS receiving unit 5. Below, the observation object which the observation part 21 observes is shown. In the following, the vehicle speed, the vehicle angular speed, the vehicle position, and the vehicle direction are exemplified as observation targets.
  • v PLS Vehicle speed observed based on output from vehicle speed sensor 6 ⁇
  • GYR Vehicle angular speed observed based on output from gyro sensor 7 x
  • GPS Vehicle observed based on output from GPS receiver 5 x-coordinate y
  • GPS position y coordinates theta GPS position of the vehicle to be observed on the basis of the output from the GPS receiver 5: vehicle observed on the basis of the output from the GPS receiver 5 azimuth
  • r k is the observed noise (mean 0, normal distribution N with R k is the error covariance matrix (0, R k)).
  • the Kalman filter will be described by dividing it into prediction processing for predicting the vehicle state quantity and estimation processing for estimating the vehicle state quantity.
  • k is given indicates a predicted value at time k + 1 predicted based on information up to time k.
  • k ⁇ 1 is given indicates a predicted value at time k predicted based on information up to time k ⁇ 1.
  • k + 1 is given indicates an estimated value at time k + 1 estimated based on information up to time k + 1.
  • k is given indicates the estimated value at time k estimated based on the information up to time k.
  • k ⁇ 1 is assigned indicates an estimated value at time k ⁇ 1 estimated based on information up to time k ⁇ 1.
  • the prediction process is a process of calculating a predicted value of the vehicle state quantity (hereinafter referred to as “vehicle state predicted value”) and an error covariance matrix of the vehicle state predicted value.
  • vehicle state predicted value a predicted value of the vehicle state quantity
  • error covariance matrix a predicted value of the vehicle state predicted value.
  • the predicted vehicle state value is calculated based on Equation (3).
  • the calculation of the error covariance matrix indicates calculation of the values of the respective components of the error covariance matrix.
  • Formula (3) shows calculation of the predicted vehicle state value at time k + 1 predicted based on information up to time k.
  • This predicted vehicle state value is calculated from the estimated value of the state quantity of the vehicle at time k estimated based on information up to time k, as shown on the right side of equation (3).
  • k indicating the predicted value of the x-coordinate of the vehicle position is an estimated value of the x-coordinate (x k
  • T represents an interval at which the observation unit 21 observes the observation amount based on outputs from the vehicle speed sensor 6, the gyro sensor 7, and the acceleration sensor 8.
  • the predicted vehicle state value at time k predicted based on the information up to time k ⁇ 1 can be calculated by an equation in which the time is lowered step by step in equation (3). That is, the predicted vehicle state value at time k predicted based on the information up to time k ⁇ 1 is calculated based on the estimated value of the state quantity of the vehicle at time k ⁇ 1 estimated based on the information up to time k ⁇ 1. Is done.
  • the error covariance matrix of the predicted vehicle state value is calculated based on Equation (4).
  • the error covariance matrix is a matrix of variance and covariance related to the state quantity of the vehicle.
  • Variance is the square of the error. That is, the variance of the vehicle state quantity is the square of the vehicle state quantity error. Therefore, calculating the error covariance matrix of the predicted vehicle state value is equivalent to calculating the error of the predicted vehicle state value.
  • Equation (4) P represents an error covariance matrix.
  • the left side of Equation (4) represents an error covariance matrix at time k + 1 predicted based on information up to time k.
  • the error covariance matrix shown on the left side of Equation (4) is calculated based on the error covariance matrix at time k estimated based on information up to time k.
  • F represents a Jacobian matrix obtained from the state equation of Expression (1).
  • a subscript T in F indicates a transposed matrix.
  • Q denotes an error covariance matrix of system noise.
  • the formula of subtracting the time of the P subscript in Formula (4) by one step It can be calculated. That is, the error covariance matrix of the predicted vehicle state at time k predicted based on the information up to time k ⁇ 1 is the vehicle state quantity estimated at time k ⁇ 1 based on the information up to time k ⁇ 1. It is calculated by the error covariance matrix of the estimated value.
  • the prediction process for example, when predicting the state quantity of the vehicle at time k, the predicted vehicle state value at time k calculated based on the information up to time k ⁇ 1 and the information up to time k ⁇ 1.
  • the error covariance matrix of the predicted vehicle state value at time k calculated based on the above is calculated. That is, the prediction process predicts the probability distribution of the state quantity of the vehicle.
  • the estimation process is expressed as an estimated value of the vehicle state quantity (hereinafter referred to as “vehicle condition estimated value”) based on the predicted vehicle state value calculated in the prediction process and the error covariance matrix of the predicted vehicle state value. And the error covariance matrix of the estimated vehicle state value.
  • vehicle condition estimated value an estimated value of the vehicle state quantity
  • the observation residual is calculated by equation (5).
  • the observation residual is an error between the value of the observation target and the value corresponding to the observation target calculated from the vehicle state predicted value.
  • the left side is an observation residual vector in which the observation residual is represented in vector.
  • the first term on the right side is an observation target vector to be observed by the observation unit 21.
  • the second term on the right side is obtained by multiplying the predicted vehicle state value predicted by the prediction process by H, which is an observation matrix obtained from the observation equation.
  • the vehicle state estimated value is calculated by Equation (6) using the observation residual shown in Equation (5).
  • Equation (6) represents the estimated vehicle state value at time k + 1 predicted based on information up to time k + 1.
  • This vehicle state estimated value is calculated by correcting the vehicle state predicted value at time k + 1 estimated based on the information up to time k from the observation residual, as shown on the right side of equation (6).
  • the predicted vehicle state value at time k predicted based on the information up to time k is corrected by the observation residual. It is calculated by.
  • Expression (6) represents an expression for correcting the vehicle state predicted value using the observation residual and calculating the vehicle state estimated value.
  • K k is used as a correction coefficient when correcting the predicted vehicle state value with the observation residual.
  • K k is called the Kalman gain is expressed by the formula (7).
  • Equation (7) R k is an error covariance matrix of observation noise.
  • the subscript “ ⁇ 1” indicates an inverse matrix.
  • the Kalman gain K k shown in Expression (7) is calculated based on the error covariance matrix of the predicted vehicle state value at time k + 1 based on information up to time k.
  • the Kalman gain K k determines whether the vehicle state estimated value is calculated with emphasis on the vehicle state predicted value or the observation target value observed by the observation unit 21. It is a coefficient.
  • the vehicle state estimation value is the observation target because the error is sufficiently small. It is desirable to have a value of. This is because the vehicle state estimated value becomes a value with a sufficiently small error, that is, a highly accurate value. That is, if the value of R k is the error covariance matrix is sufficiently small, the vehicle state estimation value, it is desirable that the value to be observed.
  • the vehicle state estimation value is the vehicle state prediction value. It is desirable to be a value. This is because the estimated vehicle state value has a value that is sufficiently smaller than the error to be observed, that is, a highly accurate value. That is, when the error covariance matrix of the vehicle state predicted value is sufficiently smaller than the value of R k , the vehicle state estimated value is preferably the vehicle state predicted value.
  • the Kalman gain K k is set so that the vehicle state estimated value becomes an appropriate value in accordance with R k that is an error covariance matrix of observation noise and the error covariance matrix of the vehicle state predicted value. It is a coefficient to do.
  • the Kalman gain K k can be set to the vehicle speed sensor 6 and the gyro if the error covariance matrix of the vehicle state predicted value is accurately predicted.
  • the vehicle state estimation value is set to an appropriate value according to the error covariance matrix of the observation target based on the output from the sensor 7 and the GPS receiving unit 5.
  • the error covariance matrix of the estimated vehicle state value is calculated by equation (8).
  • the error covariance matrix is a matrix of variance and covariance regarding the state quantity of the vehicle in this embodiment.
  • the variance of the vehicle state quantity is the square of the vehicle state quantity error. Therefore, calculating the error covariance matrix of the estimated vehicle state value corresponds to calculating the error of the estimated vehicle state value.
  • Equation (8) P represents an error covariance matrix, as in equation (4).
  • I shows a unit matrix.
  • the left side of Equation (8) shows an error covariance matrix at time k + 1 estimated based on information up to time k + 1.
  • the error covariance matrix shown on the left side of Equation (8) is calculated based on the error covariance matrix at time k + 1 predicted based on information up to time k.
  • the error covariance matrix of the estimated vehicle state value at the time k predicted based on the information up to the time k it can be calculated by the equation in which the time of the subscript P is lowered step by step in the equation (8). . That is, the error covariance matrix of the vehicle state estimated value at time k predicted based on the information up to time k is the error covariance matrix of the vehicle state estimated value at time k estimated based on the information up to time k ⁇ 1. Is calculated by
  • Equation (8) is an error covariance matrix of the vehicle state estimated value, which is obtained by multiplying the error covariance matrix of the vehicle state predicted value by (I ⁇ K k H). As shown in Expression (8), the error covariance matrix of the vehicle state estimated value depends on the value of the Kalman gain K k .
  • the error covariance matrix of the vehicle state estimate vehicle state prediction value Error covariance matrix This indicates that the error in the estimated vehicle state value is an error in the predicted vehicle state value with a sufficiently small error.
  • the Kalman gain K k is appropriately calculated according to the error covariance matrix of the observed noise and the error covariance matrix of the vehicle state predicted value.
  • the Kalman gain K k based on the accuracy of the precision and the vehicle state predicting value of the observation noise, the error covariance matrix of the vehicle state estimation value is set to be appropriate.
  • the estimation process for example, when estimating the state quantity of the vehicle at time k, the time calculated based on the vehicle state estimated value at time k calculated based on the information up to time k and the information up to time k. An error covariance matrix of the vehicle state estimation value at k is calculated. In other words, the estimation process estimates the vehicle state quantity establishment distribution based on the vehicle state quantity establishment distribution predicted by the prediction process.
  • the estimation unit 22 estimates the vehicle state by calculating the vehicle state estimated value and the error covariance matrix of the vehicle state estimated value by the Kalman filter.
  • the Kalman gain K k is a coefficient that appropriately sets the vehicle state estimated value and the error covariance matrix of the vehicle state estimated value. In other words, the accuracy of estimation of the state of the vehicle will depend on the Kalman gain K k.
  • the Kalman gain K k is a coefficient that appropriately sets the vehicle state estimated value and the error covariance matrix of the vehicle state estimated value according to the observation noise and the error covariance matrix of the vehicle state predicted value. is there. Therefore, in order to appropriately calculate the vehicle state estimated value and the error covariance matrix of the vehicle state estimated value, it is necessary to accurately calculate the error covariance matrix of the vehicle state predicted value.
  • the system noise and the observation noise are white noises.
  • the estimation unit 22 of the present embodiment estimates the state of the vehicle using the following Kalman filter.
  • the estimation of the vehicle state by the estimation unit 22 will be described through the operation of the navigation device 1 when estimating the vehicle state.
  • FIG. 2 is a flowchart showing the operation of the navigation device 1.
  • the observation unit 21 of the navigation device 1 observes the vehicle speed, the vehicle angular velocity, and the vehicle acceleration based on signals output from the vehicle speed sensor 6, the gyro sensor 7, and the acceleration sensor 8 (step SA1). .
  • the observation unit 21 observes the speed of the vehicle, the angular velocity of the vehicle, and the acceleration of the vehicle every time the vehicle speed sensor 6, the gyro sensor 7, and the acceleration sensor 8 output signals. That is, the interval at which the observation unit 21 observes these is the same as the intervals detected by the vehicle speed sensor 6, the gyro sensor 7, and the acceleration sensor 8.
  • the observation unit 21 observes the position of the vehicle and the direction of the vehicle based on the output of the GPS reception unit 5 (step SA2).
  • the observation unit 21 observes the position of the vehicle and the direction of the vehicle each time information indicating the position of the vehicle and information indicating the direction of the vehicle are output from the GPS receiving unit 5. That is, the interval at which the observation unit 21 observes the position of the vehicle and the direction of the vehicle is the interval at which the GPS reception unit 5 receives GPS radio waves.
  • the estimation unit 22 of the navigation device 1 executes a vehicle state estimation process for estimating the vehicle state based on the observation target observed by the observation unit 21 (step SA3).
  • FIG. 3 is a flowchart showing the operation of the estimation unit 22 in the vehicle state estimation process.
  • the estimation part 22 performs a prediction process (step SB1).
  • the prediction process is a process of calculating the vehicle state prediction value and the error covariance matrix of the vehicle state prediction value.
  • the vehicle state predicted value is calculated by the equation (3).
  • the error covariance matrix of the predicted vehicle state value is calculated by equation (9).
  • F k is a Jacobian matrix obtained from the state equation of Expression (1), and is represented by Expression (10).
  • Q k is an error covariance matrix of the system noise and is expressed by Expression (11).
  • C k is a covariance matrix between the error of the estimated vehicle state value estimated last time and the error of the observation amount that the observation unit 21 observes based on the outputs from the vehicle speed sensor 6 and the gyro sensor 7. ).
  • Qk + 2 ⁇ Ck is the system noise.
  • ⁇ k vPLS represents an error in the vehicle speed calculated by the observation unit 21.
  • ⁇ k ⁇ GYR indicates an error of the vehicle that the observation unit 21 calculates angular velocity.
  • ⁇ k vPLS is an error accumulated in the moving distance obtained from this speed as time passes.
  • ⁇ k ⁇ GYR is an error accumulated in the azimuth change amount obtained from this angular velocity with time, as in ⁇ k vPLS .
  • the vehicle speed error calculated by the observation unit 21 based on the output from the vehicle speed sensor 6 is the vehicle position. Is input as an error. That is, ⁇ k vPLS is input as cos ( ⁇ k
  • ⁇ k vPLS is input as a vehicle position error
  • ⁇ k ⁇ GYR is input as a vehicle heading error.
  • the estimation unit 22 calculates an error covariance matrix of the vehicle state predicted value.
  • the sigma k VPLS Kalman filter type as an error of the position of the vehicle, will be described to enter the ⁇ k ⁇ GYR as an error of the azimuth of the vehicle.
  • ⁇ k vPLS inputting ⁇ k vPLS to the Kalman filter as an error in the position of the vehicle will be described in detail.
  • the state quantity of the vehicle will be exemplified as the x coordinate of the vehicle position and the speed of the vehicle, and ⁇ k vPLS is input to the Kalman filter as an error of the vehicle position.
  • the x coordinate of the position of the vehicle is determined by the speed of the vehicle.
  • the observation target is the speed of the vehicle.
  • the error covariance matrix of the vehicle state estimated value at time k estimated based on the information up to time k is expressed by Expression (4), Expression (6), and Expression (8).
  • k) is expressed by equation (15).
  • k) is an error covariance matrix of estimated vehicle state values at time k estimated based on information up to time k.
  • k) represents an error variance of the position of the vehicle.
  • k) represents an error variance of the vehicle speed.
  • k) represents an error covariance between the position of the vehicle and the speed of the vehicle.
  • each of F, Q, H, and R is represented by equations (16) to (19).
  • F is a Jacobian matrix obtained from the state equation.
  • Q is a dispersion matrix of system noise.
  • H is a Jacobian matrix obtained from the observation equation.
  • R is a dispersion matrix of observation noise. Since the observation amount is the vehicle speed, r k is the error of the speed of the vehicle. That, r k 2 shows the error variance of the speed of the vehicle which the observation unit 21 observes.
  • Equation (21) is transformed into Equation (22) when p 11 (k
  • k-1 ) is the error variance of the predicted values of the position of the vehicle at time k based on the information up to time k-1.
  • Equation (23) is a coefficient that is multiplied by p 11 (k
  • Coefficients shown in equation (24) is "1 (correlation coefficient of the error of the error and the vehicle speed position of the vehicle) 2.” This indicates that the coefficient shown in Equation (24) is a non-deterministic coefficient of the linear regression model using the least square method. In other words, the smaller the correlation between the vehicle position error and the vehicle speed error, the larger the coefficient, and the greater the variance of the predicted vehicle position value. On the other hand, the greater the correlation, the smaller the coefficient, and the smaller the variance of the predicted value of the vehicle position. Equation (22) also shows that the variance in the estimated value of the vehicle position decreases exponentially with respect to the vehicle speed error.
  • the error variance of the estimated value of the vehicle position is determined based on the correlation between the error of the vehicle position and the error of the vehicle speed. It also indicates that the error in the estimated value of the vehicle position does not accumulate due to the error in the vehicle speed. That is, in order to accumulate p 11 (k
  • the speed error of the vehicle is input to the Kalman filter as a position error of the vehicle having a calculus relationship with the speed of the vehicle.
  • the correlation between the vehicle speed error and the vehicle position error can be reduced by the amount of the speed error accumulated in the moving distance, and the accumulated vehicle speed error is accumulated as the vehicle position error.
  • the error covariance matrix of the predicted value of the vehicle position can be calculated. That is, the error covariance matrix of the predicted value of the vehicle position can be calculated in consideration of the accumulated vehicle speed error.
  • Equation (4) cannot be used as it is.
  • Equation (4) the first term on the right side converts the error covariance matrix of the previously estimated vehicle state value into the error covariance matrix of the current vehicle state quantity using the Jacobian matrix obtained from the state equation. ing.
  • the second term on the right side is an error covariance matrix of system noise. That is, Equation (4) is obtained by adding the error covariance matrix of the system noise to the error covariance matrix of the previous vehicle state estimated value converted into the error covariance matrix of the current vehicle state estimated value, It is a formula for calculating an error covariance matrix of vehicle state predicted values.
  • the proposition shown in the equation (25) is used in the equation (4).
  • Equation (25) represents an equation that holds for the variance Var () when the random variable X and the random variable Y are independent of each other. That is, in the equation (4) used when calculating the error covariance matrix of the vehicle state prediction value in the Kalman filter, the error covariance matrix of the vehicle state quantity and the error covariance matrix of the system noise are independent from each other. It is assumed that. However, when the error covariance matrix of the system noise is used as the error covariance matrix of the accumulated error, the error covariance matrix of the vehicle state predicted value cannot be accurately calculated using Equation (4). This is because the error covariance matrix of the vehicle state quantity and the error covariance matrix of the accumulated error input as the vehicle state quantity are not independent.
  • the position error of the vehicle is determined as the speed error accumulated over time
  • the position error of the vehicle and the speed error of the vehicle are variables that change with one change, That is, they are variables that are not independent of each other. Therefore, it is necessary to use a proposition that holds when the random variable X and the random variable Y are not independent.
  • the proposition is expressed by equation (26).
  • the vehicle speed error is input to the Kalman filter as the vehicle position error.
  • the description about inputting the angular velocity error to the Kalman filter as an azimuth error can be similarly explained.
  • the error of the observed quantity is input as the error of the state quantity that is in the calculus relation with the observed quantity, and the expression is independent of the error of the observed quantity and the error of the vehicle state quantity.
  • the error covariance matrix of the predicted vehicle state value can be calculated in consideration of the accumulated error.
  • the estimation unit 22 executes the estimation process.
  • the estimation unit 22 calculates a vehicle state estimated value and an error covariance matrix of the vehicle state estimated value based on the equations (5) to (8).
  • the Kalman gain K k appropriately represents the vehicle state estimation value and the vehicle state estimation value error covariance matrix according to the observation noise error covariance matrix and the vehicle prediction value error covariance matrix.
  • the coefficient to set since the estimation unit 22 can calculate the error covariance matrix of the vehicle state prediction value considering the accumulated error, the Kalman gain K k can be accurately calculated. Therefore, the estimation unit 22 can calculate the vehicle state estimated value and the error of the vehicle state estimated value with high accuracy.
  • the error covariance matrix of the vehicle state estimate is calculated by multiplying the error covariance matrix of the vehicle state predicting values (I-K k H). Therefore, since the error covariance matrix of the predicted vehicle state value is an error covariance matrix that takes into account the accumulated error, the error covariance matrix that takes into account the error that also accumulates the error covariance matrix of the calculated vehicle state estimated value It is.
  • the estimation part 22 can calculate the error covariance matrix of the vehicle state prediction value in consideration of the accumulated error, the Kalman gain K k can be accurately calculated, and the vehicle state can be accurately estimated by the Kalman filter.
  • the vehicle state is estimated by the Kalman filter, it is not necessary to estimate the vehicle state separately from the error, as compared with the configuration in which the vehicle state is estimated based on the Kalman filter whose error is to be estimated. It is possible to estimate the state of the vehicle in consideration of the error accumulated in.
  • the estimation unit 22 calculates an error covariance matrix of the predicted value of the vehicle speed based on the Kalman filter in which the error in the vehicle acceleration is input as the error in the vehicle speed. It is good also as a structure which estimates the speed of a vehicle. As a result, an error covariance matrix of the predicted value of the vehicle speed in consideration of the accumulated acceleration error is calculated, so that the Kalman gain K k can be accurately calculated, and the vehicle speed can be accurately estimated by the Kalman filter.
  • the estimation unit 22 considers the position of the vehicle and the direction of the vehicle observed by the observation unit 21 based on the information indicating the position of the vehicle output from the GPS reception unit 5 and the information indicating the direction of the vehicle.
  • the vehicle state estimation value and the error covariance matrix of the vehicle state estimation value are calculated to estimate the vehicle state.
  • the configuration is not limited to the configuration in which the estimation unit 22 estimates the state of the vehicle in consideration of the position of the vehicle observed by the observation unit 21 and the direction of the vehicle. That is, the estimating unit 22 may estimate the state of the vehicle without being based on the output from the GPS receiving unit 5.
  • the estimation unit 22 is based on an observation equation having no component of the vehicle position and vehicle orientation based on the output from the GPS receiver 5. Estimate the state quantity of.
  • the estimation unit 22 calculates the vehicle state prediction value based on the interval between the signals output from the vehicle speed sensor 6 and the gyro sensor 7. Calculate the error covariance matrix. Moreover, the estimation part 22 calculates a vehicle state estimated value based on the vehicle state estimated value estimated last time and the said space
  • the estimation unit 22 can estimate the state of the vehicle without depending on the GPS radio wave reception interval of the GPS reception unit 5. This indicates that the estimation unit 22 estimates the state of the vehicle based on the detection intervals of the vehicle speed sensor 6, the gyro sensor 7, and the acceleration sensor 8. Therefore, when the estimation unit 22 has a detection interval of the vehicle speed sensor 6, the gyro sensor 7, and the acceleration sensor 8 shorter than the reception interval of the GPS reception unit 5, the following effects are obtained. That is, from the configuration in which the vehicle state is estimated by the Kalman filter based on the reception interval, the vehicle state can be executed with high frequency for estimation, and the vehicle state can be determined without depending on the reception environment of the GPS receiver 5. Can be estimated.
  • control unit 2 of the navigation device 1 evaluates a link to be a map matching target based on the estimated vehicle state.
  • FIG. 4 is a diagram showing a road R1 and a road R2 on the map in order to explain the map matching process based on the estimated vehicle state.
  • a road R1 is a road extending in the direction Y1.
  • the road R2 is a road extending in the direction Y2 that is not parallel to the direction Y1.
  • a link L1 is a link corresponding to the road R1.
  • the link L2 is a link corresponding to the road R2.
  • the mark ⁇ is a mark indicating the position of the vehicle estimated by the estimation unit 22.
  • the estimation unit 22 estimates that the vehicle is located at the position M1 and is traveling in the traveling direction X1.
  • the position M1 is a position on the road R1, and is a position spaced to the right from the center in the width direction of the road R1 in the direction Y1.
  • the control unit 2 first refers to the map data 3a, and is a map matching candidate.
  • a map matching candidate link that is a link to be acquired is acquired.
  • the control unit 2 is positioned as a map matching candidate link within a predetermined range set in advance from the estimated position of the vehicle, and one or a plurality of azimuth errors between the vehicle and the link are within the predetermined range. Get a link.
  • the link L1 and the link L2 are located in a predetermined range from the estimated position M1 of the vehicle, and the azimuth error between the vehicle and the link is a link within the predetermined range. . Therefore, the control unit 2 acquires the link L1 and the link L2 as map matching candidate links.
  • control unit 2 calculates an evaluation amount for evaluating the link for each acquired map matching candidate link.
  • the evaluation amount is based on the error between the estimated position of the vehicle and the position of the map matching candidate link and the error between the estimated direction of the vehicle and the direction of the map matching candidate link. Is a value calculated by.
  • the error between the estimated position of the vehicle and the position of the map matching candidate link is the difference between the perpendicular and the map matching candidate link when the perpendicular is extended from the estimated position of the vehicle to the map matching candidate link. It is the difference in the x-axis direction and the y-axis direction between the intersection and the estimated vehicle position.
  • ⁇ x represents the difference between the x coordinate of the intersection and the estimated x coordinate of the vehicle position.
  • ⁇ y indicates the difference between the y coordinate of the intersection and the estimated y coordinate of the vehicle position.
  • the error in the estimated position of the vehicle and the position of the link L1 is the perpendicular line S1 extending from the position M1 to the link L1 and the intersection MM1 of the link L1 and the x-axis direction of the position M1. And the difference in the y-axis direction.
  • the error in the position between the estimated vehicle position and the link L2 is the perpendicular line S2 extending from the position M1 to the link L2, the intersection MM2 of the link L2, and the x-axis direction of the position M1. And the difference in the y-axis direction.
  • the error between the estimated vehicle orientation and the map matching candidate link orientation is the difference between the angle corresponding to the estimated vehicle orientation and the angle corresponding to the map matching candidate link orientation.
  • the direction of the vehicle means the direction of the traveling direction of the vehicle.
  • the estimated vehicle orientation is the direction of the traveling direction X1.
  • the angle corresponding to the azimuth of the vehicle means a counterclockwise separation angle between the direction toward the east and the direction of the vehicle with respect to the direction toward the east.
  • the angle corresponding to the estimated azimuth of the vehicle is the angle ⁇ 1 when the virtual straight line K1 is a virtual straight line extending east-west.
  • the direction of the link means the direction in which the link extends.
  • the direction in which the link extends means a direction corresponding to a direction in which the vehicle can travel on a road corresponding to the link, out of two directions along the link.
  • the angle corresponding to the direction of the link means a counterclockwise separation angle between the direction toward the east and the direction of the link with respect to the direction toward the east.
  • the direction of the link L1 is the direction of the direction Z1. Further, the angle corresponding to the direction of the link L1 is the angle ⁇ 2 when the virtual straight line K2 is a virtual straight line extending east-west. In the example of FIG. 4, the direction of the link L2 is the direction of the direction Z2. Further, the angle corresponding to the direction of the link L2 is the angle ⁇ 3 when the virtual straight line K3 is a virtual straight line extending east-west.
  • the azimuth error between the vehicle and the link L1 is the difference between the angle ⁇ 1 and the angle ⁇ 2.
  • the azimuth error between the vehicle and the link L2 is a difference between the angle ⁇ 1 and the angle ⁇ 3. That is, in the example of FIG. 4, in the equation (27), ⁇ represents the difference between the angle ⁇ 1 and the angle ⁇ 2, or the difference between the angle ⁇ 1 and the angle ⁇ 3.
  • the evaluation amount ⁇ is obtained by dividing the value obtained by squaring ⁇ x by the value obtained by squaring ⁇ x, dividing the value obtained by squaring ⁇ y by the value obtained by squaring ⁇ y, and ⁇ by The sum of the squared value and ⁇ divided by the squared value is calculated.
  • ⁇ x indicates an error in the position of the vehicle in the x-axis direction estimated by the estimation unit 22. That is, ⁇ x 2 indicates the variance of the position of the vehicle in the x-axis direction estimated by the estimation unit 22.
  • ⁇ y indicates an error in the position of the vehicle in the y-axis direction estimated by the estimation unit 22.
  • ⁇ y 2 indicates the variance of the position in the y-axis direction of the vehicle estimated by the estimation unit 22.
  • ⁇ represents an error in the direction of the vehicle estimated by the estimation unit 22.
  • ⁇ 2 indicates the variance of the vehicle azimuth estimated by the estimation unit 22.
  • ⁇ x 2 , ⁇ y 2 , and ⁇ 2 are variances included in the error covariance matrix of the vehicle state estimation value estimated by the estimation unit 22.
  • the evaluation amount is a value obtained by making the dimensionless by the error of the vehicle position where the error between the vehicle position and the link position is estimated, and the error between the vehicle direction and the link direction. It is a value calculated by summing with a value made dimensionless by a position error.
  • control unit 2 calculates the evaluation amount of the link L1 and the evaluation amount of the link L2 acquired as the map matching candidate links.
  • the evaluation amount of the link L1 is ⁇ 1
  • the difference in x coordinate between the position M1 and the intersection MM1 is ⁇ x1
  • the difference in y coordinate between the position M1 and the intersection MM1 is ⁇ y1
  • the difference between the angle ⁇ 1 and the angle ⁇ 2 is ⁇ 1.
  • the evaluation amount ⁇ 1 is expressed by the following equation (28).
  • ⁇ 1 ⁇ x1 2 / ⁇ x 2 + ⁇ y1 2 / ⁇ y 2 + ⁇ 1 2 / ⁇ 2 (28)
  • the evaluation amount of the link L2 is ⁇ 2
  • the difference of the x coordinate between the position M1 and the intersection MM2 is ⁇ x2
  • the difference of the y coordinate between the position M1 and the intersection MM2 is ⁇ y2
  • the difference between the angle ⁇ 1 and the angle ⁇ 3 is
  • the evaluation amount ⁇ 2 is expressed by the following equation (29).
  • ⁇ 2 ⁇ x2 2 / ⁇ x 2 + ⁇ y2 2 / ⁇ y 2 + ⁇ 2 2 / ⁇ 2 (29)
  • the distance l1 is smaller than the distance l2. That is, the difference between the x coordinate and the y coordinate between the position M1 and the intersection MM2 is smaller than the difference between the x axis coordinate and the y coordinate between the position M1 and the intersection MM2.
  • the difference between the angle ⁇ 1 and the angle ⁇ 2 is smaller than the difference between the angle ⁇ 1 and the angle ⁇ 3.
  • the value of ⁇ 1, which is the evaluation amount of the link L1 is smaller than the value of ⁇ 2, which is the evaluation amount of the link L2. Therefore, the control unit 2 determines the link L1 having a small evaluation amount as a link that associates the current position of the vehicle.
  • the control unit 2 performs evaluation of the link to be subjected to map matching based on the estimated vehicle state. As described above, when evaluating the link, the control unit 2 evaluates the link based on the variance included in the error covariance matrix of the vehicle state estimation value estimated by the estimation unit 22.
  • the error covariance matrix is a matrix calculated based on the error covariance matrix of the vehicle state predicted value calculated in consideration of accumulated errors, and is a matrix including errors calculated accurately. Therefore, the control unit 2 can accurately evaluate the link by using the variance included in the error covariance matrix for link evaluation.
  • the navigation device 1 (on-vehicle device) includes the observation unit 21 that observes an observation amount related to vehicle fluctuations, and the estimation unit 22 that estimates a state quantity indicating the state of the vehicle using a Kalman filter. .
  • the estimation unit 22 calculates a predicted value of the state quantity of the vehicle, and predicts an error covariance matrix (error) by a Kalman filter in which the observed quantity error is input as an error of the state quantity in a calculus relation. Based on the calculated predicted value and the error covariance matrix of the predicted value, an estimated value of the vehicle state quantity and an error covariance matrix of the estimated value are calculated by the Kalman filter.
  • the Kalman gain K k can be accurately calculated, the state of the vehicle can be accurately estimated by the Kalman filter.
  • the estimation unit 22 uses the Kalman filter to which the covariance between the error of the vehicle state estimation value calculated last time and the error of the observation amount input as the error of the vehicle state quantity is input, to thereby generate an error covariance matrix of the predicted values. Is calculated.
  • equation (4) uses a proposition that assumes that the error covariance matrix of the state quantity and the error covariance matrix of the system noise are independent from each other.
  • the error covariance matrix of the system noise is used as the error covariance matrix of the accumulated error
  • the error covariance matrix of the vehicle state quantity and the error covariance matrix of the accumulated error are not independent. Therefore, Expression (9) according to Expression (26), that is, an expression in which a covariance between the error of the estimated vehicle state value estimated last time and the error of the observation amount input as the state amount error is input.
  • the observation unit 21 observes the speed of the vehicle based on the output from the vehicle speed sensor 6 that detects the speed of the vehicle.
  • the estimation unit 22 calculates an error covariance matrix of predicted values using a Kalman filter to which an error in vehicle speed is input as an error in vehicle position.
  • the accumulated vehicle speed error can be taken into account, and the vehicle position can be accurately estimated by the Kalman filter.
  • the observation unit 21 observes the angular velocity of the vehicle based on the output from the gyro sensor 7 (angular velocity sensor) that detects the angular velocity of the vehicle.
  • the estimation unit 22 calculates an error covariance matrix of predicted values using a Kalman filter into which an error in vehicle angular velocity is input as an error in vehicle orientation.
  • the accumulated vehicle angular velocity error can be taken into account, and the vehicle azimuth can be accurately estimated by the Kalman filter.
  • the observation unit 21 observes the acceleration of the vehicle based on the output from the acceleration sensor 8 that detects the acceleration of the vehicle.
  • the estimation unit 22 calculates an error covariance matrix of predicted values using a Kalman filter into which an error in vehicle acceleration is input as an error in vehicle speed.
  • the accumulated vehicle acceleration error can be taken into account, and the vehicle speed can be accurately estimated by the Kalman filter.
  • FIG. 1 is a schematic diagram showing the functional configuration of the navigation device 1 classified according to main processing contents in order to facilitate understanding of the present invention. Accordingly, it can be classified into more components. Moreover, it can also classify
  • the processing units in the flowcharts of FIGS. 2 and 3 are divided according to the main processing contents in order to make the processing of the control unit 2 easy to understand.
  • the present invention is not limited by or name.
  • the processing of the control unit 2 may be divided into more processing units according to the processing content.
  • the vehicle-mounted type is illustrated as the navigation device 1, but the form of the navigation device 1 is arbitrary, and may be a portable device carried by a pedestrian, for example.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Probability & Statistics with Applications (AREA)
  • Computer Hardware Design (AREA)
  • Mathematical Physics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Navigation (AREA)

Abstract

カルマンフィルタにより車両の状態を精度よく推定できるようにすることを目的とする。　ナビゲーション装置１は、センサーからの出力に基づき、車両の変動に関する観測量を観測する観測部２１と、カルマンフィルタにより、車両の状態を示す状態量を推定する推定部２２と、を備え、推定部２２は、車両の状態量の予測値を算出し、観測量と微積分の関係にある状態量の誤差として観測量の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出し、算出した予測値と予測値の誤差共分散行列とに基づいて、カルマンフィルタにより車両の状態量の推定値と推定値の誤差共分散行列とを算出する。

Description

車載装置、及び、推定方法
　本発明は、車載装置、及び、推定方法に関する。
　本技術分野の背景技術として、特開２０００－５５６７８号公報（特許文献１）がある。特許文献１には、「車速センサ４、ジャイロ６での検出値から移動距離演算部１１、方位変化量演算部１２にて算出される移動距離、方位変化量に基づき、相対軌跡演算部１３、絶対位置演算部１４が算出する推定航法データ（車速、絶対方位、絶対位置）と、ＧＰＳ受信機８からのＧＰＳ測位データ（速度、方位、位置）との差を観測値とするカルマンフィルタ（誤差推定部１５）を備え、その状態量の一つとして、ジャイロ出力から角速度への変換ゲインの誤差（ゲイン誤差）を設定する。このカルマンフィルタにより求められるゲイン誤差の推定値により、ジャイロ出力及び変換ゲインを用いて算出された方位変化量を補正する。」と記載されている。
特開２０００－５５６７８号公報
　ところで、車速パルスやジャイロ等のセンサーからの出力に基づき観測される速度や角速度等の観測量から求められる移動距離や方位変化量は、時間の経過により誤差が累積する場合がある。特許文献１では、累積する誤差を考慮したカルマンフィルタにより車両の状態を推定する技術について開示が無く、カルマンフィルタにより車両の状態を精度よく推定できない。
　そこで、本発明は、カルマンフィルタにより車両の状態を精度よく推定できるようにすることを目的とする。
　上記目的を達成するために、車載装置は、車両に搭載される車載装置であって、センサーからの出力に基づき、前記車両の変動に関する観測量を観測する観測部と、カルマンフィルタにより、前記車両の状態を示す状態量を推定する制御部と、を備え、前記制御部は、前記車両の前記状態量の予測値を算出し、前記観測量と微積分の関係にある前記状態量の誤差として前記観測量の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出し、算出した前記予測値と前記予測値の誤差とに基づいて、前記カルマンフィルタにより前記車両の前記状態量の推定値と前記推定値の誤差とを算出することを特徴とする。
　本発明によれば、カルマンフィルタにより車両の状態を精度よく推定できる。
図１は、ナビゲーション装置の構成を示すブロック図である。 図２は、ナビゲーション装置の動作を示すフローチャートである。 図３は、車両状態推定処理における推定部の動作を示すフローチャートである。 図４は、推定した車両の状態に基づくマップマッチングの処理を説明するための図である。
　図１は、ナビゲーション装置１（車載装置）の構成を示すブロック図である。
　ナビゲーション装置１は、車両に搭載される車載装置であり、車両に搭乗しているユーザの操作に従って、地図の表示や、地図における車両の現在位置の表示、目的地までの経路探索、経路案内等を実行する。なお、ナビゲーション装置１は、車両のダッシュボード等に固定されてもよいし、車両に対し着脱可能なものであっても良い。
　図１に示すように、ナビゲーション装置１は、制御部２と、記憶部３と、タッチパネル４と、ＧＰＳ受信部５と、車速センサー６（センサー）と、ジャイロセンサー７（角速度センサー、センサー）と、加速度センサー８（センサー）とを備える。
　制御部２は、ＣＰＵや、ＲＯＭ、ＲＡＭ、その他の制御回路等を備え、ナビゲーション装置１の各部を制御する。制御部２は、ＲＯＭや記憶部３等に記憶された制御プログラムを実行することにより、後述する観測部２１、及び、推定部２２として機能する。
　記憶部３は、ハードディスクや、ＥＥＰＲＯＭ等の不揮発性メモリを備え、データを書き換え可能に記憶する。記憶部３は、制御部２が実行する制御プログラムのほか、地図データ３ａを記憶する。地図データ３ａは、交差点やその他の道路網上の接続点を示すノードに関するノード情報や、ノードとノードとの道路区間を示すリンクに関するリンク情報、地図の表示に係る情報等を有する。リンク情報は、リンク毎に、少なくとも、リンクの位置に関する情報と、リンクの方位に関する情報とを含む。
　タッチパネル４は、表示パネル４ａと、タッチセンサー４ｂとを備える。表示パネル４ａは、液晶ディスプレイやＥＬ（Electro Luminescent）ディスプレイ等により構成され、制御部２の制御の下、各種情報を表示パネル４ａに表示する。タッチセンサー４ｂは、表示パネル４ａに重ねて配置され、ユーザのタッチ操作を検出し、タッチ操作された位置を示す信号を制御部２に出力する。制御部２は、タッチセンサー４ｂからの入力に基づいて、タッチ操作に対応する処理を実行する。
　ＧＰＳ受信部５は、ＧＰＳアンテナ５ａを介してＧＰＳ衛星からのＧＰＳ電波を受信し、ＧＰＳ電波に重畳されたＧＰＳ信号から、車両の位置と、車両の進行方向の方角（以下、「車両の方位」と表現する）とを演算により取得する。ＧＰＳ受信部５は、車両の位置を示す情報、及び、車両の方位を示す情報を制御部２に出力する。
　車速センサー６は、車両の車速を検出して、検出した車速を示す信号を制御部２に出力する。
　ジャイロセンサー７は、例えば振動ジャイロにより構成され、車両の回転による角速度を検出する。ジャイロセンサー７は、検出した角速度を示す信号を制御部２に出力する。
　加速度センサー８は、車両に作用する加速度（例えば、進行方向に対する車両の傾き）を検出する。加速度センサー８は、検出した加速度を示す信号を制御部２に出力する。
　図１に示すように、制御部２は、観測部２１と、推定部２２とを備える。
　観測部２１は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８から出力される信号に基づき、車両の変動に関する観測量を観測する。本実施形態では、観測部２１は、車速センサー６から出力される車速を示す信号に基づき、観測量として車両の速度（観測量）を観測する。観測部２１は、所定の演算により、車速を示す信号から、車両の速度と車両の速度の誤差とを算出する。この算出された車両の速度の誤差は、この速度から求めた移動距離に累積する誤差である。
　また、観測部２１は、ジャイロセンサー７から出力される角速度を示す信号に基づき、観測量として車両の回転による角速度（観測量）を観測する。観測部２１は、所定の演算により、角速度を示す信号から、車両の角速度と車両の角速度の誤差とを算出する。この算出された車両の角速度の誤差は、この角速度から求めた方位変化量に累積する誤差である。
　また、観測部２１は、加速度センサー８から出力される加速度（観測量）を示す信号に基づき、観測量として車両の加速度を観測する。観測部２１は、所定の演算により、加速度を示す信号から、車両の加速度と車両の加速度の誤差とを算出する。この算出された車両の加速度の誤差は、この加速度から求めた速度に累積する誤差である。
　また、観測部２１は、ＧＰＳ受信部５から出力された車両の位置を示す情報と車両の方位を示す情報とに基づき、車両の位置と車両の方位とを観測する。
　推定部２２は、観測部２１が観測する車両の速度、車両の加速度、車両の角速度、車両の位置、及び、車両の方位に基づき、カルマンフィルタにより、車両の状態を示す状態量を推定する。本実施形態では、推定部２２は、車両の位置、車両の方位、車両の速度、及び、車両の角速度を、車両の状態量として推定する。
　後述するが、制御部２は、推定部２２が推定した車両の状態量に基づき、マップマッチングの対象となるリンクの評価を実行する。
　なお、推定部２２が推定する車両の速度は、車両の状態量に相当する。また、観測部２１が観測する車両の速度は、観測量に相当する。同様に、推定部２２が推定する車両の角速度は、車両の状態量に相当する。また、観測部２１が観測する車両の角速度は、観測量に相当する。
　ここで、カルマンフィルタによる基本的な車両の状態量の推定について説明する。
　本実施形態では、カルマンフィルタにより推定する車両の状態量は、車両の位置、車両の方位、車両の速度、及び、車両の角速度である。以下に、推定する車両の各状態量を示す。
　ｘ：車両の位置のｘ座標
　ｙ：車両の位置のｙ座標
　θ：車両の方位
　ｖ：車両の速度
　ω：車両の角速度
　ここで、車両の状態量をベクトル表記した状態ベクトルを、（ｘ、ｙ、θ、ｖ、ω）とすると、車両の状態量についての状態方程式は、式（１）で表される。
Figure JPOXMLDOC01-appb-M000001
　添え字のｋは、時刻を示す。例えば、式（１）の左辺である（ｘｋ＋１、ｙｋ＋１、θｋ＋１、ｖｋ＋１、ωｋ＋１）は、時刻ｋ＋１における車両の状態量を示す。式（１）において、右辺の第２項であるｑｋは、システム雑音（平均０、誤差共分散行列であるＱｋを有する正規分布Ｎ（０、Ｑｋ））である。誤差共分散行列とは、分散と共分散との行列である。
　本実施形態では、観測部２１は、観測対象として、車両の速度、車両の角速度、車両の加速度、車両の位置、及び、車両の方位を観測する。前述した通り、観測部２１は、車両の速度について、車速センサー６からの出力に基づき観測する。また、観測部２１は、車両の角速度について、ジャイロセンサー７からの出力に基づき観測する。また、観測部２１は、車両の加速度について、加速度センサー８からの出力に基づき観測する。また、観測部２１は、車両の位置、及び、車両の方位について、ＧＰＳ受信部５からの出力に基づき観測する。以下に、観測部２１が観測する観測対象を示す。なお、以下では、観測対象として車両の速度、車両の角速度、車両の位置、及び、車両の方位を例示する。
　ｖＰＬＳ：車速センサー６からの出力に基づき観測される車両の速度
　ωＧＹＲ：ジャイロセンサー７からの出力に基づき観測される車両の角速度
　ｘＧＰＳ：ＧＰＳ受信部５からの出力に基づき観測される車両の位置のｘ座標
　ｙＧＰＳ：ＧＰＳ受信部５からの出力に基づき観測される車両の位置のｙ座標
　θＧＰＳ：ＧＰＳ受信部５からの出力に基づき観測される車両の方位
　ここで、上記の観測対象をベクトル表記した観測ベクトルを（ｖＰＬＳ、ωＧＹＲ、ｘＧＰＳ、ｙＧＰＳ、θＧＰＳ）とすると、観測対象についての観測方程式は、式（２）で表される。
Figure JPOXMLDOC01-appb-M000002
　式（２）において、ｒｋは、観測雑音（平均０、誤差共分散行列であるＲｋを有する正規分布Ｎ（０、Ｒｋ））である。
　以下、カルマンフィルタについて、車両の状態量を予測する予測処理と、車両の状態量を推定する推定処理とに分けて説明する。
　なお、以下の説明において、添え字のｋ＋１｜ｋが付与された値は、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における予測値を示す。また、添え字のｋ｜ｋ－１が付与された値は、時刻ｋ－１までの情報に基づき予測された時刻ｋにおける予測値を示す。また、添え字のｋ＋１｜ｋ＋１が付与された値は、時刻ｋ＋１までの情報に基づき推定された時刻ｋ＋１における推定値を示す。また、添え字のｋ｜ｋが付与された値は、時刻ｋまでの情報に基づき推定された時刻ｋにおける推定値を示す。また、添え字のｋ－１｜ｋ－１が付与された値は、時刻ｋ－１までの情報に基づき推定された時刻ｋ－１における推定値を示す。
＜予測処理＞
　カルマンフィルタにおいて予測処理は、車両の状態量の予測値（以下、「車両状態予測値」と表現する）と、車両状態予測値の誤差共分散行列と、を算出する処理である。車両状態予測値は、式（３）に基づき算出される。なお、誤差共分散行列の算出とは、誤差共分散行列の各成分の値の算出を示す。
Figure JPOXMLDOC01-appb-M000003
　式（３）では、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における車両状態予測値の算出を示す。この車両状態予測値は、式（３）の右辺に示すように、時刻ｋまでの情報に基づき推定された時刻ｋにおける車両の状態量の推定値により算出される。例えば、車両の位置のｘ座標の予測値を示すｘｋ＋１｜ｋは、時刻ｋまでの情報に基づき推定された時刻ｋにおける、車両の位置のｘ座標（ｘｋ｜ｋ）の推定値と、車両の方位（θｋ｜ｋ）の推定値と、車両の速度（ｖｋ｜ｋ）の推定値とに基づき算出される。なお、式（３）において、Ｔは、車速センサー６、ジャイロセンサー７、及び、加速度センサー８からの出力に基づき観測部２１が観測量を観測する間隔を示す。
　なお、時刻ｋ－１までの情報に基づき予測された時刻ｋの車両状態予測値を算出する場合、式（３）において時刻を１ステップずつ下げた式により算出できる。つまり、時刻ｋ－１までの情報に基づき予測された時刻ｋにおける車両状態予測値は、時刻ｋ－１までの情報に基づき推定された時刻ｋ－１における車両の状態量の推定値に基づき算出される。
　車両状態予測値の誤差共分散行列は、式（４）に基づき算出される。誤差共分散行列は、本実施形態において、車両の状態量に関する分散及び共分散の行列である。分散は、誤差を二乗したものである。つまり、車両の状態量の分散は、車両の状態量の誤差を二乗したものである。したがって、車両状態予測値の誤差共分散行列を算出することは、車両状態予測値の誤差を算出することに相当する。
Figure JPOXMLDOC01-appb-M000004
　式（４）において、Ｐは、誤差共分散行列を示す。式（４）の左辺は、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における誤差共分散行列を示す。式（４）の左辺に示す誤差共分散行列は、時刻ｋまでの情報に基づき推定された時刻ｋにおける誤差共分散行列に基づき算出される。なお、Ｆは、式（１）の状態方程式から求められるヤコビ行列を示す。また、Ｆにおける添え字のＴは、転置行列を示す。また、Ｑは、システム雑音の誤差共分散行列を示す。
　なお、時刻ｋ－１までの情報に基づき予測された時刻ｋにおける車両状態予測値の誤差共分散行列を算出する場合、式（４）においてＰの添え字の時刻を１ステップずつ下げた式により算出できる。つまり、時刻ｋ－１までの情報に基づき予測された時刻ｋにおける車両状態予測値の誤差共分散行列は、時刻ｋ－１までの情報に基づき推定された時刻ｋ－１における車両の状態量の推定値の誤差共分散行列により算出される。
　このように、予測処理では、例えば、時刻ｋにおける車両の状態量を予測する場合、時刻ｋ－１までの情報に基づき算出された時刻ｋにおける車両状態予測値と、時刻ｋ－１までの情報に基づき算出された時刻ｋにおける車両状態予測値の誤差共分散行列とを算出する。つまり、予測処理は、車両の状態量の確立分布を予測している。
＜推定処理＞
　次に、推定処理について説明する。
　カルマンフィルタにおいて推定処理は、予測処理にて算出した車両状態予測値、及び、車両状態予測値の誤差共分散行列に基づいて、車両の状態量の推定値（以下、「車両状態推定値」と表現する）と、車両状態推定値の誤差共分散行列とを算出する処理である。
　推定処理では、式（５）により観測残差が算出される。観測残差とは、観測対象の値と、車両状態予測値から算出される観測対象に対応する値との誤差である。
Figure JPOXMLDOC01-appb-M000005
　式（５）において、左辺は、観測残差をベクトル表記した観測残差ベクトルである。また、式（５）において、右辺の第１項は、観測部２１の観測対象とする観測対象ベクトルである。また、式（５）において、右辺の第２項は、予測処理で予測した車両状態予測値に、観測方程式から求められる観測行列であるＨを掛けたものである。
　車両状態推定値は、式（５）に示す観測残差を用いて、式（６）により算出される。
Figure JPOXMLDOC01-appb-M000006
　式（６）では、時刻ｋ＋１までの情報に基づき予測された時刻ｋ＋１における車両状態推定値を示す。この車両状態推定値は、式（６）の右辺に示すように、時刻ｋまでの情報に基づき推定された時刻ｋ＋１における車両状態予測値を、観測残差より補正して算出される。なお、時刻ｋまでの情報に基づき予測された時刻ｋにおける車両状態推定値を算出する場合、時刻ｋ－１までの情報に基づき予測された時刻ｋにおける車両状態予測値を観測残差により補正することで算出される。
　前述の通り、式（６）は、観測残差を用いて、車両状態予測値を補正し、車両状態推定値を算出する式を示す。式（６）に示すように、車両状態予測値を観測残差にて補正する際の補正係数としてＫｋが用いられる。Ｋｋは、カルマンゲインと呼ばれ、式（７）で表される。
Figure JPOXMLDOC01-appb-M000007
　式（７）において、Ｒｋは、観測雑音の誤差共分散行列である。また、添え字の「－１」は、逆行列を示す。式（７）に示すカルマンゲインＫｋは、時刻ｋまでの情報に基づく時刻ｋ＋１における車両状態予測値の誤差共分散行列に基づき算出される。
　式（６）において、カルマンゲインＫｋは、車両状態推定値について、車両状態予測値を重視して算出するか、観測部２１が観測する観測対象の値を重視して算出するかを決定付ける係数である。
　例えば、車速センサー６、ジャイロセンサー７、及び、ＧＰＳ受信部５からの出力に基づく観測対象の値の誤差が充分に小さい場合、車両状態推定値は、当該誤差が充分に小さいことから、観測対象の値となることが望ましい。これは、車両状態推定値が、誤差の充分に小さい値、つまり、精度の高い値になるためである。つまり、誤差共分散行列であるＲｋの値が充分に小さい場合、車両状態推定値は、観測対象の値となることが望ましい。ここで、Ｒｋの値が充分に小さい場合のカルマンゲインＫｋをＫｋ＝Ｈ－１として式（６）に与えると、式（６）の右辺の第１項は、無くなる。つまり、車両状態推定値は、誤差が充分に小さい観測対象の値となる。
　また、例えば、車両状態予測値の誤差が、車速センサー６、ジャイロセンサー７、及び、ＧＰＳ受信部５からの出力に基づく観測対象の誤差より充分に小さい場合、車両状態推定値は、車両状態予測値となることが望ましい。これは、車両状態推定値が、観測対象の誤差より充分に誤差が小さい値、つまり、精度の高い値になるためである。つまり、車両状態予測値の誤差共分散行列が、Ｒｋの値より充分に小さい場合、車両状態推定値は、車両状態予測値となることが望ましい。ここで、車両状態予測値の誤差共分散行列がＲｋの値より充分に小さい場合のカルマンゲインＫｋとしてＫｋ＝０として式（６）に与えると、車両状態推定値は、車両状態予測値となる。
　このように、カルマンゲインＫｋは、観測雑音の誤差共分散行列であるＲｋ、及び、車両状態予測値の誤差共分散行列に応じて、車両状態推定値が適切な値となるように設定する係数である。つまり、観測雑音の誤差共分散行列が観測対象の誤差共分散行列である場合、カルマンゲインＫｋは、車両状態予測値の誤差共分散行列が正確に予測されていれば、車速センサー６、ジャイロセンサー７、及び、ＧＰＳ受信部５からの出力に基づく観測対象の誤差共分散行列に応じて、車両状態推定値が適切な値となるように設定する。
　車両状態推定値の誤差共分散行列は、式（８）により算出される。前述した通り、誤差共分散行列は、本実施形態では、車両の状態量に関する、分散及び共分散の行列である。車両の状態量の分散は、車両の状態量の誤差を二乗したものである。したがって、車両状態推定値の誤差共分散行列を算出することは、車両状態推定値の誤差を算出することに相当する。
Figure JPOXMLDOC01-appb-M000008
　式（８）において、Ｐは、式（４）と同様、誤差共分散行列を示す。また、式（８）において、Ｉは、単位行列を示す。式（８）の左辺は、時刻ｋ＋１までの情報に基づき推定された時刻ｋ＋１における誤差共分散行列を示す。式（８）の左辺に示す誤差共分散行列は、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における誤差共分散行列に基づき算出される。
　なお、時刻ｋまでの情報に基づき予測された時刻ｋにおける車両状態推定値の誤差共分散行列を算出する場合、式（８）においてＰの添え字の時刻を１ステップずつ下げた式により算出できる。つまり、時刻ｋまでの情報に基づき予測された時刻ｋにおける車両状態推定値の誤差共分散行列は、時刻ｋ－１までの情報に基づき推定された時刻ｋにおける車両状態予測値の誤差共分散行列により算出される。
　式（８）は、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列に、（Ｉ－ＫｋＨ）を掛けた式である。式（８）に示すように、車両状態推定値の誤差共分散行列は、カルマンゲインＫｋの値に依存する。
　例えば、観測雑音の誤差が充分に小さい場合のカルマンゲインＫｋとしてＫｋ＝Ｈ－１を式（８）に与えると、車両状態推定値の誤差共分散行列は零行列となる。これは、推定した車両状態推定値の誤差が充分に小さいことを示す。
　また、例えば、観測雑音の誤差より車両状態予測値の誤差が充分に小さい場合のカルマンゲインの値としてＫｋ＝０を与えることにより、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列になる。これは、車両状態推定値の誤差が、誤差の充分に小さい車両状態予測値の誤差であることを示す。
　このように、式（８）において、カルマンゲインＫｋは、観測雑音の誤差共分散行列と、車両状態予測値の誤差共分散行列に応じて、車両状態推定値の誤差共分散行列が適切になるように設定する。つまり、カルマンゲインＫｋは、観測雑音の精度と車両状態予測値の精度とに基づいて、車両状態推定値の誤差共分散行列が適切になるように設定する。
　以上、推定処理では、例えば、時刻ｋにおける車両の状態量を推定する場合、時刻ｋまでの情報に基づき算出された時刻ｋにおける車両状態推定値と、時刻ｋまでの情報に基づき算出された時刻ｋにおける車両状態推定値の誤差共分散行列とを算出する。つまり、推定処理は、予測処理で予測した車両の状態量の確立分布に基づいて、車両の状態量の確立分布を推定している。
　以上の算出により、推定部２２は、カルマンフィルタによって、車両状態推定値と、車両状態推定値の誤差共分散行列とを算出することにより、車両の状態を推定する。
　前述した通り、カルマンゲインＫｋは、車両状態推定値と、車両状態推定値の誤差共分散行列とを適切に設定する係数である。つまり、車両の状態の推定の精度は、カルマンゲインＫｋに依存する。カルマンゲインＫｋは、前述した通り、観測雑音と、車両状態予測値の誤差共分散行列とに応じて、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に設定する係数である。そのため、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に算出するためには、車両状態予測値の誤差共分散行列が、正確に算出される必要がある。しかしながら、カルマンフィルタでは、システム雑音、及び、観測雑音が白色性の雑音であることが仮定される。つまり、カルマンフィルタにおいて誤差を雑音として扱う場合、誤差の平均が０であることが前提となる。そのため、平均が０とならない誤差、すなわち、累積する誤差を考慮して、車両状態予測値の誤差共分散行列を算出できない場合があり、この場合、カルマンゲインＫｋは、正確に算出されない。これは、車両状態推定値と車両状態推定値の誤差共分散行列とが精度よく算出できない、すなわち、車両の状態を精度よく推定できないことを示す。
　そこで、本実施形態の推定部２２は、以下のカルマンフィルタにより、車両の状態を推定する。
　以下、車両の状態を推定する際のナビゲーション装置１の動作を通して、推定部２２による車両の状態の推定について説明する。
　図２は、ナビゲーション装置１の動作を示すフローチャートである。
　ナビゲーション装置１の観測部２１は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８から出力される信号に基づき、車両の速度、車両の角速度、及び、車両の加速度を観測する（ステップＳＡ１）。観測部２１は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８が信号を出力する度に、車両の速度、車両の角速度、及び、車両の加速度を観測する。つまり、観測部２１がこれらを観測する間隔は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８が検出する間隔と同じ間隔である。
　次いで、観測部２１は、ＧＰＳ受信部５の出力に基づき、車両の位置、及び、車両の方位を観測する（ステップＳＡ２）。観測部２１は、車両の位置を示す情報と車両の方位を示す情報とがＧＰＳ受信部５から出力される度に、車両の位置、及び、車両の方位を観測する。つまり、観測部２１が車両の位置及び車両の方位を観測する間隔は、ＧＰＳ受信部５がＧＰＳ電波を受信する間隔となる。
　次いで、ナビゲーション装置１の推定部２２は、観測部２１が観測した観測対象に基づき、車両の状態を推定する車両状態推定処理を実行する（ステップＳＡ３）。
　図３は、車両状態推定処理における推定部２２の動作を示すフローチャートである。
　推定部２２は、予測処理を実行する（ステップＳＢ１）。前述した通り、予測処理は、車両状態予測値、及び、車両状態予測値の誤差共分散行列を算出する処理である。車両状態予測値は、式（３）によって算出される。車両状態予測値の誤差共分散行列は、式（９）によって算出される。
Figure JPOXMLDOC01-appb-M000009
　式（９）において、Ｆｋは、式（１）の状態方程式から求められるヤコビ行列であり、式（１０）で表される。また、Ｑｋは、システム雑音の誤差共分散行列であり、式（１１）で表される。また、Ｃｋは、前回推定した車両状態推定値の誤差と、観測部２１が車速センサー６及びジャイロセンサー７からの出力に基づき観測する観測量の誤差との共分散行列であり、式（１２）で表される。Ｑｋ＋２・Ｃｋがシステム雑音である。
Figure JPOXMLDOC01-appb-M000010
Figure JPOXMLDOC01-appb-M000011
Figure JPOXMLDOC01-appb-M000012
　式（１２）において、σｋ ｖＰＬＳは、観測部２１が算出する車両の速度の誤差を示す。また、σｋ ωＧＹＲは、観測部２１が算出する車両の角速度の誤差を示す。前述した通り、σｋ ｖＰＬＳは、時間の経過と共に、この速度から求めた移動距離に累積する誤差である。また、σｋ ωＧＹＲは、σｋ ｖＰＬＳと同様、時間と共に、この角速度から求めた方位変化量に累積する誤差である。
　式（１２）に示すように、車両状態予測値の誤差共分散行列を算出する式（９）において、車速センサー６から出力に基づき観測部２１が算出する車両の速度の誤差は、車両の位置の誤差として入力される。すなわち、σｋ ｖＰＬＳは、ｃｏｓ（θｋ｜ｋ）σｋ ｖＰＬＳＴ、及び、ｓｉｎ（θｋ｜ｋ）σｋ ｖＰＬＳＴとして入力される。また、ジャイロセンサー７からの出力に基づき観測部２１が算出する車両の角速度の誤差は、車両の方位の誤差として入力される。すなわち、σｋ ωＧＹＲは、σｋ ωＧＹＲＴとして入力される。
　このように、時間の経過と共に累積する誤差、すなわち、σｋ ｖＰＬＳとσｋ ωＧＹＲとについて、σｋ ｖＰＬＳを車両の位置の誤差として入力し、σｋ ωＧＹＲを車両の方位の誤差として入力したカルマンフィルタにより、推定部２２は、車両状態予測値の誤差共分散行列を算出する。
　ここで、カルマンフィルタにσｋ ｖＰＬＳを車両の位置の誤差として入力し、σｋ ωＧＹＲを車両の方位の誤差として入力することについて説明する。以下では、カルマンフィルタにσｋ ｖＰＬＳを車両の位置の誤差として入力することについて詳述する。
　ここで説明の便宜上、車両の状態量を、車両の位置のｘ座標と車両の速度として例示し、カルマンフィルタにσｋ ｖＰＬＳを車両の位置の誤差として入力することについて説明する。この例では、車両の位置のｘ座標は、車両の速度により決定する。この例では、観測対象は、車両の速度である。
　したがって、車両の状態量についての状態方程式は、式（１３）で表される。また、観測対象についての観測方程式は、式（１４）で表される。
Figure JPOXMLDOC01-appb-M000013
Figure JPOXMLDOC01-appb-M000014
　式（１３）及び式（１４）に基づき、時刻ｋまでの情報に基づき推定した時刻ｋにおける車両状態推定値の誤差共分散行列を、式（４）、式（６）、及び、式（８）に基づき算出する。ここで、車両状態推定値の誤差共分散行列を算出するに際し、Ｐ（ｋ｜ｋ）を式（１５）とする。Ｐ（ｋ｜ｋ）は、時刻ｋまでの情報に基づき推定した時刻ｋにおける車両状態推定値の誤差共分散行列である。
Figure JPOXMLDOC01-appb-M000015
　式（１５）において、ｐ１１ （ｋ｜ｋ）は、車両の位置の誤差分散を示す。また、ｐ２２ （ｋ｜ｋ）は、車両の速度の誤差分散を示す。また、ｐ１２ （ｋ｜ｋ）は、車両の位置と、車両の速度との誤差共分散を示す。
　また、車両状態推定値の誤差共分散行列を算出するあたり、Ｆ、Ｑ、Ｈ、及び、Ｒのそれぞれを式（１６）～式（１９）とする。ここで、Ｆは、状態方程式から求められるヤコビ行列である。また、Ｑは、システム雑音の分散行列である。また、Ｈは、観測方程式から求められるヤコビ行列である。また、Ｒは、観測雑音の分散行列である。観測量が車両の速度であることから、ｒｋは、車両の速度の誤差である。つまり、ｒｋ ２は、観測部２１が観測する車両の速度の誤差分散を示す。
Figure JPOXMLDOC01-appb-M000016
Figure JPOXMLDOC01-appb-M000017
Figure JPOXMLDOC01-appb-M000018
Figure JPOXMLDOC01-appb-M000019
　式（１５）～式（１９）に基づき、車両状態推定値の誤差共分散行列、すなわち、Ｐ（ｋ｜Ｋ）を算出すると、車両状態推定値の誤差共分散行列は、式（２０）で表される。
Figure JPOXMLDOC01-appb-M000020
　したがって、車両の位置の推定値の誤差分散は、式（２１）で表される。
Figure JPOXMLDOC01-appb-M000021
　式（２１）は、共通因数であるｐ１１ （ｋ｜ｋ－１）でくくると、式（２２）に変形される。ｐ１１ （ｋ｜ｋ－１）は、時刻ｋ－１までの情報に基づく時刻ｋにおける車両の位置の予測値の誤差分散である。
Figure JPOXMLDOC01-appb-M000022
　式（２２）に示すように、車両の位置の推定値の誤差分散であるｐ１１ （ｋ｜ｋ）は、車両の位置の予測値の誤差分散であるｐ１１ （ｋ｜ｋ－１）に、式（２３）を掛けることで求められる。
Figure JPOXMLDOC01-appb-M000023
　式（２３）は、式（２２）に示すように、車両の位置の予測値の分散であるｐ１１ （ｋ｜ｋ－１）に掛けられる係数である。この係数は、分母に車両の速度の誤差（ｒｋ）が含まれることを除くと、式（２４）と一致する。
Figure JPOXMLDOC01-appb-M000024
　式（２４）に示す係数は、「１－（車両の位置の誤差と車両の速度の誤差との相関係数）２」である。これは、式（２４）に示す係数が、最小二乗法を用いた線形回帰モデルの非決定係数であることを示す。つまり、車両の位置の誤差と、車両の速度の誤差との相関性が小さければ小さいほど、当該係数は大きい値となり、車両の位置の予測値の分散は大きい値となる。一方で、当該相関性が大きければ大きいほど、当該係数は小さい値となり、車両の位置の予測値の分散は小さい値となる。また、式（２２）は、車両の位置の推定値の分散が、車両の速度の誤差に対して指数関数的に減少することを示す。
　したがって、車両の位置の推定値の誤差分散は、車両の位置の誤差と、車両の速度の誤差との相関性に基づき決定されていることを示している。また、車両の位置の推定値の誤差は、車両の速度の誤差によって累積しないことも示す。つまり、車両の速度の誤差の累積に伴い、車両の位置の推定値の誤差分散であるｐ１１ （ｋ｜ｋ）を累積させるためには、車両の位置の誤差と、車両の速度の誤差との相関性を小さくする必要がある。つまり、ｐ１１ （ｋ｜ｋ）を累積させるためには、カルマンフィルタにおいて、車両の位置の誤差と、車両の速度の誤差との相関性を小さくする必要がある。具体的には、移動距離に累積する速度の誤差に関しては、カルマンフィルタに車両の速度の誤差を入力し、車両の位置の誤差を推定するのではなく、カルマンフィルタに車両の位置の誤差として車両の速度の誤差を入力し、車両の位置の誤差を推定する必要がある。
　したがって、式（１２）に示すように、移動距離に累積する速度の誤差に関しては、車両の速度の誤差を、車両の速度と微積分の関係にある車両の位置の誤差としてカルマンフィルタに入力する。これによって、移動距離に累積する速度の誤差の分だけ、車両の速度の誤差と車両の位置の誤差との相関性を小さくでき、累積する車両の速度の誤差を累積する車両の位置の誤差として、車両の位置の予測値の誤差共分散行列を算出できる。つまり、累積する車両の速度の誤差を考慮した、車両の位置の予測値の誤差共分散行列を算出できる。
　しかしながら、車両の位置の誤差として車両の速度の誤差をカルマンフィルタに入力する際、式（４）をそのまま用いることができない。
　式（４）において、右辺の第１項は、前回に推定した車両状態推定値の誤差共分散行列を、状態方程式から求められるヤコビ行列により今回の車両の状態量の誤差共分散行列に変換している。一方、右辺の第２項は、システム雑音の誤差共分散行列である。つまり、式（４）は、前回の車両状態推定値の誤差共分散行列を今回の車両状態推定値の誤差共分散行列に変換したものに、システム雑音の誤差共分散行列を加算することで、車両状態予測値の誤差共分散行列を算出する式である。ここで、式（４）は、式（２５）に示す命題が用いられている。
Figure JPOXMLDOC01-appb-M000025
　式（２５）は、確率変数Ｘと確率変数Ｙとが互いに独立である場合に、分散Ｖａｒ（）について成立する式を示す。つまり、カルマンフィルタにおいて車両状態予測値の誤差共分散行列を算出する際に用いられる式（４）は、車両の状態量の誤差共分散行列と、システム雑音の誤差共分散行列とが互いに独立であることが前提となっている。しかしながら、システム雑音の誤差共分散行列を累積する誤差の誤差共分散行列とする場合、式（４）では、車両状態予測値の誤差共分散行列が正確に算出できない。これは、車両の状態量の誤差共分散行列と、車両の状態量として入力した累積する誤差の誤差共分散行列とが独立でないためである。例えば、車両の位置の誤差が時間の経過で累積する速度の誤差に決定されることから、車両の位置の誤差と、車両の速度の誤差とは、一方の変化に伴い一方も変化する変数、すなわち、互いに独立しない変数である。そのため、確率変数Ｘと確率変数Ｙとが独立でない場合に成立する命題を用いる必要がある。当該命題は、式（２６）により表される。
Figure JPOXMLDOC01-appb-M000026
　式（２６）において、Ｃｏｖ（）は共分散を示す。したがって、式（２４）と比較すると、式（２６）は、確率変数Ｘと確率変数Ｙとの共分散が入力されている。
　式（２６）に従った式（９）を用いることにより、車両の状態量の誤差共分散行列と、累積する誤差の誤差共分散行列との独立性をなくして、正確に車両状態予測値の誤差共分散行列を算出できる。
　以上、車両の速度の誤差を車両の位置の誤差としてカルマンフィルタに入力することを説明した。角速度の誤差を方位の誤差としてカルマンフィルタに入力することについての説明も、同様に説明できる。
　以上の説明のように、観測量と微積分の関係にある状態量の誤差として、観測量の誤差が入力され、また、観測量の誤差と、車両の状態量の誤差との独立性のない式により、累積する誤差を考慮した、車両状態予測値の誤差共分散行列を算出できる。
　図３に示すフローチャートの説明に戻り、推定部２２は、予測処理を実行すると、推定処理を実行する。推定部２２は、式（５）～式（８）に基づいて、車両状態推定値と、車両状態推定値の誤差共分散行列とを算出する。
　前述の通り、カルマンゲインＫｋは、観測雑音の誤差共分散行列と、車両予測値の誤差共分散行列とに応じて、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に設定する係数である。予測処理において、推定部２２は、累積する誤差を考慮した車両状態予測値の誤差共分散行列を算出できるため、カルマンゲインＫｋを正確に算出できる。したがって、推定部２２は、車両状態推定値と車両状態推定値の誤差とを精度よく算出できる。
　また、式（８）に示すように、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列に（Ｉ－ＫｋＨ）を掛けて算出される。したがって、車両状態予測値の誤差共分散行列は、累積する誤差を考慮した誤差共分散行列であるため、算出される車両状態推定値の誤差共分散行列も累積する誤差を考慮した誤差共分散行列である。
　このように、推定部２２は、累積する誤差を考慮した車両状態予測値の誤差共分散行列を算出できるため、カルマンゲインＫｋが正確に算出でき、カルマンフィルタにより精度よく車両の状態を推定できる。
　また、カルマンフィルタで車両の状態を推定しているため、誤差を推定の対象としたカルマンフィルタに基づき車両の状態を推定する構成と比較し、誤差とは別に車両の状態を推定する必要がなく、容易に累積する誤差を考慮した車両の状態を推定できる。
　なお、状態量として車両の速度を推定する場合、推定部２２は、車両の加速度の誤差を車両の速度の誤差として入力したカルマンフィルタに基づき車両の速度の予測値の誤差共分散行列を算出し、車両の速度を推定する構成としてもよい。これにより、累積する加速度の誤差を考慮した車両の速度の予測値の誤差共分散行列が算出されるため、カルマンゲインＫｋを正確に算出でき、カルマンフィルタにより車両の速度を精度よく推定できる。
　上述した通り、推定部２２は、ＧＰＳ受信部５が出力する車両の位置を示す情報と車両の方位を示す情報とに基づき、観測部２１が観測した車両の位置と車両の方位とを加味して、車両状態推定値と車両状態推定値の誤差共分散行列とを算出し、車両の状態を推定する構成である。しかしながら、観測部２１が観測する車両の位置と車両の方位とを加味して、推定部２２が車両の状態を推定する構成に限定されない。すなわち、推定部２２は、ＧＰＳ受信部５からの出力に基づくことなく、車両の状態を推定してもよい。
　例えば、車両がＧＰＳ電波をＧＰＳ受信部５により受信できない環境下にいる場合、推定部２２は、ＧＰＳ受信部５からの出力に基づく車両の位置及び車両の方位の成分のない観測方程式に基づき車両の状態量を推定する。
　式（１２）に示すように、推定部２２は、車両状態予測値の誤差共分散行列を算出する際、車速センサー６及びジャイロセンサー７から出力される信号の間隔に基づき、車両状態予測値の誤差共分散行列を算出する。また、推定部２２は、式（３）に示すように、車両状態予測値を、前回推定した車両状態推定値と当該間隔に基づき算出する。したがって、推定部２２は、車両状態予測値、及び、車両状態予測値の誤差共分散行列の算出に際し、ＧＰＳ受信部５の受信間隔に依存しない。したがって、推定部２２は、式（５）において、ＧＰＳ受信部５からの出力に基づく観測対象の成分のない式に基づくことにより、車両状態推定値と車両状態推定値の誤差共分散行列とを算出できる。
　つまり、推定部２２は、ＧＰＳ受信部５のＧＰＳ電波の受信の間隔に依存せず、車両の状態を推定できる。これは、推定部２２が車速センサー６、ジャイロセンサー７、及び、加速度センサー８の検出間隔に基づき、車両の状態を推定することを示す。したがって、推定部２２が車速センサー６、ジャイロセンサー７、及び、加速度センサー８の検出間隔が、ＧＰＳ受信部５の受信間隔より短い場合において以下の効果を奏する。すなわち、当該受信間隔に基づいてカルマンフィルタにより車両の状態を推定する構成より、車両の状態を推定について高い頻度で実行でき、また、ＧＰＳ受信部５の受信環境に依存することなく、車両の状態を推定できる。
　前述した通り、ナビゲーション装置１の制御部２は、推定した車両の状態に基づき、マップマッチングの対象となるリンクの評価を実行する。
　図４は、推定した車両の状態に基づくマップマッチングの処理を説明するため、地図上の道路Ｒ１と、道路Ｒ２とを示す図である。図４において、道路Ｒ１は、方向Ｙ１に向かって延在する道路である。また、道路Ｒ２は、方向Ｙ１と並行でない方向Ｙ２に向かって延在する道路である。
　図４において、リンクＬ１は、道路Ｒ１に対応するリンクである。また、リンクＬ２は、道路Ｒ２に対応するリンクである。
　また、図４において、マークαは、推定部２２により推定された車両の位置を示すマークである。以下、図４を用いた説明では、推定部２２により、車両が、位置Ｍ１に位置し、進行方向Ｘ１に向かって走行しているものと推定されたとする。図４に示すように、位置Ｍ１は、道路Ｒ１上の位置であって、道路Ｒ１の幅方向の中心から、方向Ｙ１に向かって右側に離間した位置である。
　道路Ｒ１と、道路Ｒ２と、推定された車両の位置との関係が、図４に示す関係である場合、マップマッチングに際し、まず、制御部２は、地図データ３ａを参照し、マップマッチングの候補となるリンクであるマップマッチング候補リンクを取得する。制御部２は、マップマッチング候補リンクとして、推定された車両の位置から予め設定された所定の範囲内に位置し、かつ、車両とリンクとの方位の誤差が所定範囲内である１又は複数のリンクを取得する。図４の例では、リンクＬ１、及び、リンクＬ２は、推定された車両の位置である位置Ｍ１から所定範囲に位置し、また、車両とリンクとの方位の誤差が所定範囲内のリンクである。このため、制御部２は、リンクＬ１、及び、リンクＬ２をマップマッチング候補リンクとして取得する。
　次いで、制御部２は、取得したマップマッチング候補リンクのそれぞれについて、リンクを評価する評価量を算出する。
　評価量は、推定された車両の位置とマップマッチング候補リンクの位置との誤差、及び、推定された車両の方位とマップマッチング候補リンクの方位との誤差に基づいて、下記に示す式（２７）により算出される値である。
　τ＝δｘ２／Δｘ２＋δｙ２／Δｙ２＋δθ２／Δθ２・・・（２７）
　ここで、推定された車両の位置とマップマッチング候補リンクの位置との誤差とは、推定された車両の位置からマップマッチング候補リンクに対して垂線を延ばした場合における垂線及びマップマッチング候補リンクとの交点と、推定された車両の位置との、ｘ軸方向、及び、ｙ軸方向における差である。式（２７）において、δｘは、当該交点のｘ座標と、推定された車両の位置のｘ座標との差を示す。また、δｙは、当該交点のｙ座標と、推定された車両の位置のｙ座標との差を示す。
　図４の例において、推定された車両の位置とリンクＬ１との位置の誤差は、位置Ｍ１からリンクＬ１に対して延ばした垂線Ｓ１及びリンクＬ１の交点ＭＭ１と、位置Ｍ１とのｘ軸方向、及び、ｙ軸方向における差である。また、図４において、推定された車両の位置とリンクＬ２との位置の誤差は、位置Ｍ１からリンクＬ２に対して延ばした垂線Ｓ２及びリンクＬ２の交点ＭＭ２と、位置Ｍ１とのｘ軸方向、及び、ｙ軸方向における差である。
　また、推定された車両の方位とマップマッチング候補リンクの方位との誤差とは、推定された車両の方位に対応する角度と、マップマッチング候補リンクの方位に対応する角度との差である。
　前述した通り、車両の方位とは、車両の進行方向の方角を意味する。図４の例では、推定された車両の方位は、進行方向Ｘ１の方角である。また、車両の方位に対応する角度とは、東に向かう方向を基準として、東に向かう方向と、車両の方位との反時計回りの離間角度を意味する。図４の例では、推定された車両の方位に対応する角度は、仮想直線Ｋ１が東西に延びる仮想直線であるとした場合に、角度θ１である。
　また、リンクの方位とは、リンクが延在する方向の方角を意味する。リンクが延在する方向とは、リンクに沿った２つの方向のうち、リンクに対応する道路において車両が走行可能な方向に対応する方向を意味する。また、リンクの方位に対応する角度とは、東に向かう方向を基準として、東に向かう方向と、リンクの方位との反時計回りの離間角度を意味する。
　図４の例では、リンクＬ１の方位は、方向Ｚ１の方角である。また、リンクＬ１の方位に対応する角度は、仮想直線Ｋ２が東西に延びる仮想直線であるとした場合に、角度θ２である。また、図４の例では、リンクＬ２の方位は、方向Ｚ２の方角である。また、リンクＬ２の方位に対応する角度は、仮想直線Ｋ３が東西に延びる仮想直線であるとした場合に、角度θ３である。
　図４の例において、車両とリンクＬ１との方位誤差は、角度θ１と角度θ２との差である。また、車両とリンクＬ２との方位誤差は、角度θ１と角度θ３との差である。つまり、図４の例において、式（２７）では、δθは、角度θ１と角度θ２との差、または、角度θ１と角度θ３との差を示す。
　式（２７）に示すように、評価量τは、δｘを二乗した値にΔｘを二乗した値で割ったものと、δｙを二乗した値にΔｙを二乗した値で割ったものと、δθを二乗した値にΔθを二乗した値で割ったものと、の和により算出される。Δｘは、推定部２２が推定した車両のｘ軸方向の位置の誤差を示す。つまり、Δｘ２は、推定部２２が推定した車両のｘ軸方向の位置の分散を示す。また、Δｙは、推定部２２が推定した車両のｙ軸方向の位置の誤差を示す。つまり、Δｙ２は、推定部２２が推定した車両のｙ軸方向の位置の分散を示す。また、Δθは、推定部２２が推定した車両の方位の誤差を示す。つまり、Δθ２は、推定部２２が推定した車両の方位の分散を示す。Δｘ２、Δｙ２、及び、Δθ２は、推定部２２が推定した車両状態推定値の誤差共分散行列が含む分散である。このように、評価量は、車両の位置とリンクの位置との誤差を推定した車両の位置の誤差により無次元化した値と、車両の方位とリンクの方位との誤差を、推定した車両の位置の誤差により無次元化した値との和により算出される値である。
　図４の例では、制御部２は、マップマッチング候補リンクとして取得したリンクＬ１の評価量とリンクＬ２の評価量とを算出する。
　リンクＬ１の評価量をτ１とし、位置Ｍ１と交点ＭＭ１とのｘ座標の差をδｘ１とし、位置Ｍ１と交点ＭＭ１とのｙ座標の差をδｙ１とし、角度θ１と角度θ２との差をδθ１とした場合、評価量τ１は、下記に示す式（２８）で表される。
　τ１＝δｘ１２／Δｘ２＋δｙ１２／Δｙ２＋δθ１２／Δθ２・・・（２８）
　一方、リンクＬ２の評価量をτ２とし、位置Ｍ１と交点ＭＭ２とのｘ座標の差をδｘ２とし、位置Ｍ１と交点ＭＭ２とのｙ座標の差をδｙ２とし、角度θ１と角度θ３との差をδθ２とした場合、評価量τ２は、下記に示す式（２９）で表される。
　τ２＝δｘ２２／Δｘ２＋δｙ２２／Δｙ２＋δθ２２／Δθ２・・・（２９）
　図４の例では、距離ｌ２より距離ｌ１が小さい。すなわち、位置Ｍ１と交点ＭＭ２とにおけるｘ軸座標及びｙ座標の差より、位置Ｍ１と交点ＭＭ２とにおけるｘ座標及びｙ座標の差が小さい。また、図４の例では、角度θ１と角度θ３との差より角度θ１と角度θ２との差が小さい。このため、リンクＬ１の評価量であるτ１の値の方が、リンクＬ２の評価量であるτ２の値よりも小さくなる。したがって、制御部２は、評価量が小さいリンクＬ１を、車両の現在位置を対応付けるリンクとして決定する。
　このように、制御部２は、推定した車両の状態に基づいて、マップマッチングの対象となるリンクの評価を実行する。上述したように、制御部２は、リンクを評価する際、推定部２２が推定した車両状態推定値の誤差共分散行列が含む分散に基づき、リンクを評価する。当該誤差共分散行列は、累積する誤差を考慮して算出された車両状態予測値の誤差共分散行列に基づき算出された行列であり、精度よく算出された誤差を含む行列である。したがって、制御部２は、当該誤差共分散行列が含む分散をリンクの評価に用いることにより、正確にリンクを評価できる。
　以上、説明したように、ナビゲーション装置１（車載装置）は、車両の変動に関する観測量を観測する観測部２１と、カルマンフィルタにより、車両の状態を示す状態量を推定する推定部２２と、を備える。推定部２２は、車両の状態量の予測値を算出し、観測量と微積分の関係にある状態量の誤差として観測量の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列（誤差）を算出し、算出した予測値と予測値の誤差共分散行列とに基づいて、カルマンフィルタにより車両の状態量の推定値と推定値の誤差共分散行列とを算出する。
　これにより、観測量の誤差を微積分の関係にある状態量の誤差として入力することにより、観測量の誤差と状態量の誤差との相関性を小さくし、累積する観測量の誤差を考慮して車両状態予測値の誤差共分散行列を算出できるため、カルマンゲインＫｋを正確に算出でき、カルマンフィルタにより車両の状態を精度よく推定できる。
　また、推定部２２は、前回算出した車両状態推定値の誤差と、車両の状態量の誤差として入力された観測量の誤差との共分散が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。
　前述した通り、式（４）は、状態量の誤差共分散行列と、システム雑音の誤差共分散行列とが互いに独立であることが前提となる命題が用いられている。しかしながら、システム雑音の誤差共分散行列を累積する誤差の誤差共分散行列とする場合、車両の状態量の誤差共分散行列と、累積する誤差の誤差共分散行列とが独立でない。そこで、式（２６）に従った式（９）、すなわち、前回推定した車両状態推定値の誤差と、状態量の誤差として入力された観測量の誤差との共分散が入力された式を用いることにより、車両の状態量の誤差共分散行列と、累積する誤差の誤差共分散行列との独立性をなくして、正確に車両状態予測値の誤差共分散行列を算出できる。
　また、観測部２１は、車両の速度を検出する車速センサー６からの出力に基づき、車両の速度を観測する。推定部２２は、車両の位置の誤差として車両の速度の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。
　これにより、車両の速度の誤差と車両の位置の誤差との相関性を小さくすることで、累積する車両の速度の誤差を考慮でき、カルマンフィルタにより車両の位置を精度よく推定できる。
　また、観測部２１は、車両の角速度を検出するジャイロセンサー７（角速度センサー）からの出力に基づき、車両の角速度を観測する。推定部２２は、車両の方位の誤差として車両の角速度の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。
　これにより、車両の角速度の誤差と車両の方位の誤差との相関性を小さくすることで、累積する車両の角速度の誤差を考慮でき、カルマンフィルタにより車両の方位を精度よく推定できる。
　観測部２１は、車両の加速度を検出する加速度センサー８からの出力に基づき、車両の加速度を観測する。推定部２２は、車両の速度の誤差として車両の加速度の誤差が入力されたカルマンフィルタにより、予測値の誤差共分散行列を算出する。
　これにより、車両の加速度の誤差と車両の速度の誤差との相関性を小さくすることで、累積する車両の加速度の誤差を考慮でき、カルマンフィルタにより車両の速度を精度よく推定できる。
　上述した実施形態は、あくまでも本発明の一態様を例示するものであって、本発明の趣旨を逸脱しない範囲で任意に変形、及び応用が可能である。
　例えば、図１は、本願発明を理解容易にするために、ナビゲーション装置１の機能構成を主な処理内容に応じて分類して示した概略図であり、ナビゲーション装置１の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、１つの構成要素がさらに多くの処理を実行するように分類することもできる。
　また、例えば、図２、及び、図３のフローチャートの処理単位は、制御部２の処理を理解容易にするために、主な処理内容に応じて分割したものであり、処理単位の分割の仕方や名称によって、本発明が限定されることはない。制御部２の処理は、処理内容に応じて、さらに多くの処理単位に分割してもよい。また、１つの処理単位がさらに多くの処理を含むように分割してもよい。
　また、例えば、上述した実施形態では、ナビゲーション装置１として車載型を例示したが、ナビゲーション装置１の形態は任意であり、例えば歩行者が携帯するポータブル型の装置でも良い。
　１　ナビゲーション装置（車載装置）
　６　車速センサー（センサー）
　７　ジャイロセンサー（センサー）
　８　加速度センサー（センサー）
　２１　観測部
　２２　推定部
 

Claims (10)

  1. 　車両に搭載される車載装置であって、
    　センサーからの出力に基づき、前記車両の変動に関する観測量を観測する観測部と、
    　カルマンフィルタにより、前記車両の状態を示す状態量を推定する推定部と、を備え、
    　前記推定部は、
    　前記車両の前記状態量の予測値を算出し、
    　前記観測量と微積分の関係にある前記状態量の誤差として前記観測量の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出し、
    　算出した前記予測値と前記予測値の誤差とに基づいて、前記カルマンフィルタにより前記車両の前記状態量の推定値と前記推定値の誤差とを算出する、
    　ことを特徴とする車載装置。
  2. 　前記推定部は、
    　前回算出した前記状態量の前記推定値の誤差と、前記状態量の誤差として入力された前記観測量の誤差との共分散が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
    　ことを特徴とする請求項１に記載の車載装置。
  3. 　前記観測部は、前記車両の速度を検出する車速センサーからの出力に基づき、前記車両の速度を観測し、
    　前記推定部は、前記車両の位置の誤差として前記車両の速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
    　ことを特徴とする請求項１又は２に記載の車載装置。
  4. 　前記観測部は、前記車両の角速度を検出する角速度センサーからの出力に基づき、前記車両の角速度を観測し、
    　前記推定部は、前記車両の方位の誤差として前記車両の角速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
    　ことを特徴とする請求項１又は２に記載の車載装置。
  5. 　前記観測部は、前記車両の加速度を検出する加速度センサーからの出力に基づき、前記車両の加速度を観測し、
    　前記推定部は、前記車両の速度の誤差として前記車両の加速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
    　ことを特徴とする請求項１又は２に記載の車載装置。
  6. 　車両の状態を示す状態量をカルマンフィルタにより推定する推定方法であって、
    　前記車両の前記状態量の予測値を算出し、
    　センサーからの出力に基づき観測された前記車両の変動に関する観測量と微積分の関係にある前記状態量の誤差として、前記観測量の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出し、
    　算出した前記予測値と前記予測値の誤差とに基づいて、前記カルマンフィルタにより前記車両の前記状態量の推定値と前記推定値の誤差とを算出する、
    　ことを特徴とする推定方法。
  7. 　前回算出した前記状態量の前記推定値の誤差と、前記状態量の誤差として入力された前記観測量の誤差との共分散が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
    　ことを特徴とする請求項６に記載の推定方法。
  8. 　前記車両の速度を検出する車速センサーからの出力に基づき、前記車両の速度を観測し、
    　前記車両の位置の誤差として前記車両の速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
    　ことを特徴とする請求項６又は７に記載の推定方法。
  9. 　前記車両の角速度を検出する角速度センサーからの出力に基づき、前記車両の角速度を観測し、
    　前記車両の方位の誤差として前記車両の角速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
    　ことを特徴とする請求項６又は７に記載の推定方法。
  10. 　前記車両の加速度を検出する加速度センサーからの出力に基づき、前記車両の加速度を観測し、
    　前記車両の速度の誤差として前記車両の加速度の誤差が入力された前記カルマンフィルタにより、前記予測値の誤差を算出する、
    　ことを特徴とする請求項６又は７に記載の推定方法。
PCT/JP2017/004510 2016-03-01 2017-02-08 車載装置、及び、推定方法 WO2017150106A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN201780014609.6A CN108700423B (zh) 2016-03-01 2017-02-08 车载装置及推定方法
US16/072,352 US11036231B2 (en) 2016-03-01 2017-02-08 In-vehicle device and estimation method
EP17759577.4A EP3425338A4 (en) 2016-03-01 2017-02-08 VEHICLE INTERNAL DEVICE CALCULATION PROCEDURE

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2016-038740 2016-03-01
JP2016038740A JP6677533B2 (ja) 2016-03-01 2016-03-01 車載装置、及び、推定方法

Publications (1)

Publication Number Publication Date
WO2017150106A1 true WO2017150106A1 (ja) 2017-09-08

Family

ID=59743868

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2017/004510 WO2017150106A1 (ja) 2016-03-01 2017-02-08 車載装置、及び、推定方法

Country Status (5)

Country Link
US (1) US11036231B2 (ja)
EP (1) EP3425338A4 (ja)
JP (1) JP6677533B2 (ja)
CN (1) CN108700423B (ja)
WO (1) WO2017150106A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2021038989A (ja) * 2019-09-03 2021-03-11 株式会社ゼンリンデータコム 車載装置、状態推定方法及びプログラム

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11136040B2 (en) * 2018-05-21 2021-10-05 Deere & Company IMU based traction control for wheeled or tracked machine
JP6581276B1 (ja) * 2018-10-18 2019-09-25 株式会社ショーワ 状態量推定装置、制御装置、および状態量推定方法
WO2020157844A1 (ja) * 2019-01-30 2020-08-06 三菱電機株式会社 計測装置、計測方法及び計測プログラム
US11874117B2 (en) * 2019-04-04 2024-01-16 Mitsubishi Electric Corporation Vehicle positioning device
CN110729982B (zh) * 2019-09-30 2023-03-10 中国船舶重工集团公司第七0七研究所 一种基于矩阵稀疏性的Kalman滤波算法优化的方法
CN111881955B (zh) * 2020-07-15 2023-07-04 北京经纬恒润科技股份有限公司 多源传感器信息融合方法及装置
EP4060614A1 (en) * 2021-03-19 2022-09-21 Aptiv Technologies Limited Method for determining noise statistics of object sensors
CN113436442B (zh) * 2021-06-29 2022-04-08 西安电子科技大学 一种利用多地磁传感器的车速估计方法
CN113792265A (zh) * 2021-09-10 2021-12-14 中国第一汽车股份有限公司 一种坡度估计方法、装置、电子设备以及存储介质
CN114018250B (zh) * 2021-10-18 2024-05-03 杭州鸿泉物联网技术股份有限公司 惯性导航方法、电子设备、存储介质和计算机程序产品
CN114689079A (zh) * 2022-03-22 2022-07-01 南斗六星系统集成有限公司 一种车辆状态判断方法、装置、设备及可读存储介质
CN115859039B (zh) * 2023-03-01 2023-05-23 南京信息工程大学 一种车辆状态估计方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000055678A (ja) 1998-08-04 2000-02-25 Denso Corp 車両用現在位置検出装置
JP2001174275A (ja) * 1999-12-17 2001-06-29 Furuno Electric Co Ltd ハイブリッド航法およびその装置
JP2008076096A (ja) * 2006-09-19 2008-04-03 Mitsubishi Electric Corp 追尾方法及びその装置
JP2014142272A (ja) * 2013-01-24 2014-08-07 Clarion Co Ltd 位置検出装置およびプログラム

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4229141B2 (ja) * 2006-06-19 2009-02-25 トヨタ自動車株式会社 車両状態量推定装置及びその装置を用いた車両操舵制御装置
JP4781300B2 (ja) * 2007-03-01 2011-09-28 アルパイン株式会社 位置検出装置および位置検出方法
JP2010117148A (ja) * 2008-11-11 2010-05-27 Seiko Epson Corp 位置算出方法及び位置算出装置
US8224519B2 (en) 2009-07-24 2012-07-17 Harley-Davidson Motor Company Group, LLC Vehicle calibration using data collected during normal operating conditions
JP5071533B2 (ja) * 2010-05-19 2012-11-14 株式会社デンソー 車両用現在位置検出装置
JP5610847B2 (ja) * 2010-05-26 2014-10-22 三菱電機株式会社 角速度推定装置及びコンピュータプログラム及び角速度推定方法
CN102410837B (zh) * 2011-07-29 2014-10-29 江苏大学 一种车辆组合定位导航系统及方法
CN102519470B (zh) * 2011-12-09 2014-05-07 南京航空航天大学 多级嵌入式组合导航系统及导航方法
JP5692044B2 (ja) * 2011-12-21 2015-04-01 トヨタ自動車株式会社 車両状態量推定装置、車両用操舵制御装置
EP3136488B1 (en) * 2013-07-02 2019-11-13 Asahi Kasei Kabushiki Kaisha Electrolyte solution and processes for preparing electrolyte membrane, electrode catalyst layer, membrane-electrode assembly, and fuel cell
US10901095B2 (en) 2014-04-25 2021-01-26 Nec Corporation Position and attitude estimation device, image processing device, and position and attitude estimation method

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000055678A (ja) 1998-08-04 2000-02-25 Denso Corp 車両用現在位置検出装置
JP2001174275A (ja) * 1999-12-17 2001-06-29 Furuno Electric Co Ltd ハイブリッド航法およびその装置
JP2008076096A (ja) * 2006-09-19 2008-04-03 Mitsubishi Electric Corp 追尾方法及びその装置
JP2014142272A (ja) * 2013-01-24 2014-08-07 Clarion Co Ltd 位置検出装置およびプログラム

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP3425338A4

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2021038989A (ja) * 2019-09-03 2021-03-11 株式会社ゼンリンデータコム 車載装置、状態推定方法及びプログラム
JP7272910B2 (ja) 2019-09-03 2023-05-12 株式会社ゼンリンデータコム 車載装置、状態推定方法及びプログラム

Also Published As

Publication number Publication date
JP2017156186A (ja) 2017-09-07
JP6677533B2 (ja) 2020-04-08
US11036231B2 (en) 2021-06-15
US20190041863A1 (en) 2019-02-07
CN108700423B (zh) 2022-02-01
EP3425338A4 (en) 2019-10-30
EP3425338A1 (en) 2019-01-09
CN108700423A (zh) 2018-10-23

Similar Documents

Publication Publication Date Title
WO2017150106A1 (ja) 車載装置、及び、推定方法
US10620320B2 (en) Position estimation system and estimation method
EP2664894B1 (en) Navigation apparatus
US8041472B2 (en) Positioning device, and navigation system
JP4199553B2 (ja) ハイブリッド航法装置
JP7272910B2 (ja) 車載装置、状態推定方法及びプログラム
WO2019188745A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
CN110346824B (zh) 一种车辆导航方法、系统、装置及可读存储介质
US20210278217A1 (en) Measurement accuracy calculation device, self-position estimation device, control method, program and storage medium
WO2018212301A1 (ja) 自己位置推定装置、制御方法、プログラム及び記憶媒体
JP2007333652A (ja) 測位装置、ナビゲーションシステム
JPH0868654A (ja) 車両用現在位置検出装置
JP4884109B2 (ja) 移動軌跡算出方法、移動軌跡算出装置及び地図データ生成方法
JPH0868652A (ja) 車両用現在位置検出装置
JP6707627B2 (ja) 測定装置、測定方法、及び、プログラム
JP4376738B2 (ja) 角速度センサのゼロ点誤差検出装置および方法
WO2017168588A1 (ja) 測定装置、測定方法、及び、プログラム
TWI587155B (zh) Navigation and Location Method and System Using Genetic Algorithm
JP2017181195A (ja) 測定装置、測定方法、及び、プログラム
JP6413816B2 (ja) 漫然運転判定装置
JP4974801B2 (ja) ナビゲーション装置
JP2018048985A (ja) 取付角算出装置、取付角評価装置、取付角算出方法および取付角評価方法
CN118226771A (zh) 用于控制自动化车辆的方法、控制设备和存储介质
CN115236708A (zh) 车辆的位置姿态状态估计方法、装置、设备及存储介质
JP2017177962A (ja) 測定装置、測定方法、及び、プログラム

Legal Events

Date Code Title Description
NENP Non-entry into the national phase

Ref country code: DE

WWE Wipo information: entry into national phase

Ref document number: 2017759577

Country of ref document: EP

ENP Entry into the national phase

Ref document number: 2017759577

Country of ref document: EP

Effective date: 20181001

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 17759577

Country of ref document: EP

Kind code of ref document: A1