WO2012153501A1 - 位置算出方法及び位置算出装置 - Google Patents

位置算出方法及び位置算出装置 Download PDF

Info

Publication number
WO2012153501A1
WO2012153501A1 PCT/JP2012/002967 JP2012002967W WO2012153501A1 WO 2012153501 A1 WO2012153501 A1 WO 2012153501A1 JP 2012002967 W JP2012002967 W JP 2012002967W WO 2012153501 A1 WO2012153501 A1 WO 2012153501A1
Authority
WO
WIPO (PCT)
Prior art keywords
calculation
unit
error
ins
result
Prior art date
Application number
PCT/JP2012/002967
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 EP12782570.1A priority Critical patent/EP2717014B1/en
Priority to US14/116,750 priority patent/US9026362B2/en
Publication of WO2012153501A1 publication Critical patent/WO2012153501A1/ja

Links

Images

Classifications

    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/03—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers
    • G01S19/05—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing aiding data
    • 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
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Definitions

  • the present invention relates to a position calculation method using the measurement results of the inertial positioning unit and the satellite positioning unit in combination.
  • inertial sensors In various fields such as so-called seamless positioning, motion sensing, and attitude control, the use of inertial sensors is attracting attention.
  • an acceleration sensor As an inertial sensor, an acceleration sensor, a gyro sensor, a pressure sensor, a geomagnetic sensor, and the like are widely known.
  • An inertial navigation system (hereinafter referred to as “INS (Inertial Navigation System)”) that performs inertial navigation calculation using the detection result of the inertial sensor has also been devised.
  • INS Inertial Navigation System
  • Patent Document 1 discloses a technique for correcting an INS calculation result using GPS (Global Positioning System).
  • the technology for correcting the INS calculation result using GPS is based on the assumption that the GPS calculation result is correct.
  • the technique disclosed in Patent Document 1 is also the same.
  • the accuracy of the GPS calculation result may decrease due to various factors such as the signal strength of the GPS satellite signal received from the GPS satellite, the reception environment, the sky arrangement of the GPS satellite, and the multipath.
  • the present invention has been made in view of the above-described problems, and an object of the present invention is to propose a method for calculating the position more accurately by using the measurement results of the satellite positioning unit and the inertial positioning unit.
  • the first mode for solving the above problems is to execute at least a first calculation process for calculating at least a position of the moving body using a measurement result of an inertial positioning unit installed in the moving body.
  • the position calculation method includes: adjusting a calculation coefficient related to the first calculation process using a result of the first calculation process and a result of the second calculation process.
  • a first calculation processing unit that executes a first calculation process for calculating the position of the moving body
  • a second calculation processing unit that executes a second calculation process for calculating the position of the moving body using the result of the first calculation process and the measurement result of the satellite positioning unit installed in the moving body.
  • an adjustment unit that adjusts the calculation coefficient related to the first calculation process using the result of the first calculation process and the result of the second calculation process. May be.
  • At least the first calculation process for calculating the position of the moving body is executed using the measurement result of the inertial positioning unit installed in the moving body. Further, the second calculation process for calculating the position of the moving body is executed using the result of the first calculation process and the measurement result of the satellite positioning unit installed in the moving body. Then, the calculation coefficient related to the first calculation process is adjusted using the result of the first calculation process and the result of the second calculation process.
  • the calculation process includes a first calculation process using the measurement result of the inertial positioning unit and a second calculation process using the result of the first calculation process and the measurement result of the satellite positioning unit. Then, by adjusting the calculation coefficient related to the first calculation process using the result of the first calculation process and the result of the second calculation process, the accuracy of position calculation in the first calculation process is improved. Can be improved.
  • the measurement result of the inertial positioning unit includes an inertial positioning position
  • the first calculation process includes the calculated first calculation position.
  • a predetermined error estimation calculation for estimating a first position error inherent in the first adjustment and the adjustment uses the first position error and the second calculation position obtained by the second calculation process.
  • a position calculation method may be configured that includes estimating an inertial positioning error inherent in the inertial positioning position and adjusting the calculation coefficient using the inertial positioning error.
  • a predetermined error estimation calculation is performed to estimate the first position error inherent in the first calculation position calculated in the first calculation process. Then, using the first position error and the second calculation position obtained in the second calculation process, an inertial positioning error inherent in the inertial positioning position included in the measurement result of the inertial positioning unit is estimated. By using the second calculation position together with the first position error, it is possible to appropriately estimate the inertial positioning error. Then, by adjusting the calculation coefficient related to the first calculation process using the inertial positioning error, it is possible to improve the accuracy of position calculation in the first calculation process.
  • the estimating includes calculating a difference between the inertial positioning position and the second calculation position, and the first position error. And calculating the inertial positioning error by averaging the difference and the difference may be configured.
  • the difference between the inertial positioning position and the second calculation position is calculated. Then, the inertial positioning error can be appropriately estimated by averaging the first position error and the difference.
  • the averaging process may be performed based on one or both of the positioning environment and the reliability of the measurement result of the satellite positioning unit.
  • a position calculation method may be configured, which is a process of setting a weight between the position error and the difference and performing a weighted average.
  • the weight of the first position error and the difference is set based on one or both of the positioning environment and the reliability of the measurement result of the satellite positioning unit. Weighted average. Thereby, based on the reliability of the measurement environment and the measurement result of the satellite positioning unit, the inertial positioning error can be estimated more accurately.
  • the measurement result of the inertial positioning unit includes an inertial positioning position
  • the first calculation process includes A position calculation method that is a Kalman filter process using the inertial positioning position as an input may be configured.
  • the position of the moving body and the like can be obtained easily and appropriately by performing the Kalman filter process using the inertial positioning position as an input as the first calculation process.
  • the measurement result of the satellite positioning unit includes a speed
  • the Kalman filter process is a process using the speed as an observation amount.
  • a position calculation method may be configured.
  • the speed included in the measurement result of the satellite positioning unit is further used as the observation amount, so that the position of the moving body can be obtained more accurately. Can do.
  • a satellite positioning position is included in the measurement result of the satellite positioning unit, and the second calculation process includes
  • the position calculation method may be configured, which is a Kalman filter process using the result of the first calculation process as an input and using the satellite positioning position as an observation amount.
  • the Kalman filter process is executed with the result of the first calculation process as an input and the satellite positioning position included in the measurement result of the satellite positioning unit as an observation amount. By doing so, the position of the moving body can be determined easily and appropriately.
  • the block diagram of a position calculation apparatus The block diagram of a 1st position calculation apparatus. Explanatory drawing of the input-output data of a 1st Kalman filter process. Explanatory drawing of the 1st weight setting condition. Explanatory drawing of 2nd weight setting conditions. The figure which shows an example of the experimental result which performed position calculation. The figure which shows an example of the experimental result which performed position calculation.
  • the system block diagram of a navigation system The block diagram which shows the function structure of a car navigation apparatus.
  • the flowchart which shows the flow of a navigation process.
  • the flowchart which shows the flow of an INS calculation error estimation process.
  • FIG. 1 is a main configuration diagram of a position calculation apparatus 1 according to the present embodiment.
  • the position calculation device 1 is a device (system) that is provided in a moving body and calculates the position of the moving body.
  • the moving body may be a person such as a car, a motorcycle, a bicycle, a ship, a train, or a person.
  • a person may carry the position calculation device 1 and the person himself / herself may include the position calculation device 1.
  • a unit (module) is illustrated by a double line, and a processing block that performs arithmetic processing using a measurement result of the unit is illustrated by a single line to distinguish the two.
  • the position calculation device 1 includes an inertial positioning unit 2 and a satellite positioning unit 3 as units (modules). In addition, the position calculation device 1 includes a first calculation processing unit 5, a second calculation processing unit 7, and a calculation coefficient adjustment unit 9 as main processing blocks.
  • the position calculation device 1 may be configured with units and processing blocks, or the position calculation device 1 may be configured with only processing blocks without the unit.
  • the inertial positioning unit 2 is a unit for performing positioning using inertial navigation.
  • the inertial positioning unit 2 includes an inertial sensor such as an acceleration sensor and a gyro sensor, an inertial measurement unit (IMU (Inertial Measurement Unit)) in which the inertial sensor is packaged, an inertial navigation system in which an inertial measurement unit and an arithmetic processing unit are packaged.
  • IMU Inertial Measurement Unit
  • INS Inertial Navigation System
  • the satellite positioning unit 3 is a unit for performing positioning using a satellite positioning system.
  • a GPS Global Positioning System
  • satellite positioning system For example, a GPS (Global Positioning System) which is a kind of satellite positioning system can be applied.
  • the first arithmetic processing unit 5 calculates the position of at least the moving body by performing a predetermined first arithmetic processing using the measurement result of the inertial positioning unit 2 and the given observation amount in combination. To do.
  • a suitable application example for the first arithmetic processing is Kalman filter processing.
  • the second calculation processing unit 7 uses the first calculation result that is the calculation result of the first calculation processing unit 5 and the measurement result of the satellite positioning unit 3 to perform a second calculation process that is determined in advance. To calculate the position of the moving body.
  • a suitable application example for the second arithmetic processing is Kalman filter processing, sigma point filter processing, or regression filter processing.
  • the calculation coefficient adjustment unit 9 uses the first calculation result input from the first calculation processing unit 5 and the second calculation result input from the second calculation processing unit 7 to perform the first calculation processing. The calculation coefficient is adjusted. The first calculation processing unit 5 performs the first calculation processing using the calculation coefficient adjusted by the calculation coefficient adjustment unit 9.
  • FIG. 2 is a configuration diagram of a first position calculation device 1A to which the position calculation device 1 of FIG. 1 is applied.
  • the first position calculation device 1A includes an INS unit 2A, a GPS unit 3A, a first Kalman filter processing unit 5A, a second Kalman filter processing unit 7A, and a Kalman filter coefficient adjustment unit 9A.
  • the first position calculation device 1A is a system in which the INS unit 2A is applied as the inertial positioning unit 2 and the GPS unit 3A is applied as the satellite positioning unit 3.
  • the first and second arithmetic processing units 5 and 7 are systems to which the first and second Kalman filter processing units 5A and 7A are applied, respectively.
  • the INS unit 2A is configured to be able to output INS measurement information such as acceleration and angular velocity measured by the IMU in the local coordinate system.
  • the INS unit 2A is configured to perform an inertial navigation calculation using the INS measurement information to calculate and output the position, speed, and attitude angle of the moving body.
  • the INS unit 2A calculates and outputs the INS calculation position (inertial positioning position), the INS calculation speed, and the INS calculation attitude angle in the absolute coordinate system.
  • the GPS unit 3A is configured to receive GPS satellite signals transmitted from GPS satellites, and to measure and output GPS measurement information such as code phase, Doppler frequency, pseudorange, and pseudorange change rate.
  • the GPS unit 3A is configured to be able to calculate and output the position and speed of the moving body by performing GPS calculation using GPS measurement information.
  • the GPS unit 3A calculates and outputs a GPS calculation position (satellite positioning position) and a GPS calculation speed using an absolute coordinate system.
  • the local coordinate system is a local coordinate system (sensor coordinate system) associated with the inertial sensor of the INS unit 2A.
  • the absolute coordinate system is a coordinate system that defines a moving space of a moving body.
  • NED North East Down
  • ENU East North Up
  • ECEF Earth Centered Earth Fixed
  • a coordinate system such as a coordinate system is applicable.
  • the INS unit 2A calculates and outputs the position, velocity, and attitude angle of the moving object in the ENU coordinate system
  • the GPS unit 3A calculates and outputs the position and velocity of the moving object in the ECEF coordinate system. It will be described as being. In the first and second Kalman filter processes, description will be made assuming that the calculation is performed in the ENU coordinate system. In addition, various quantities such as acceleration and speed are actually expressed as vectors having magnitude and direction. However, in this specification, the term “vector” is omitted, and the description will be simply made as acceleration and velocity.
  • the first Kalman filter processing unit 5A performs a calculation process based on the theory of the Kalman filter to calculate the position, speed, posture angle, etc. of the moving body. For example, an INS calculation position, an INS calculation speed, and an INS calculation attitude angle input from the INS unit 2A are set as an input U (control input). Also, a given constraint condition determined based on the GPS calculation speed input from the GPS unit 3A and the motion model of the moving object is defined as an observation amount Z. Then, the Kalman filter prediction calculation (time update) and correction calculation (observation update) are performed to obtain the state estimated value.
  • the state “X” of the moving object to be estimated is set, for example, as in the following equations (1) and (2).
  • the first Kalman filter process in this embodiment is an error estimation type Kalman filter process for estimating an error included in the calculation result of the INS unit 2A. That is, the error included in the calculation result of the INS unit 2A is estimated as the states “X 1 ” and “X 2 ”.
  • the first Kalman filter processing includes an error covariance matrix “P 11 ” including the error covariance of each component in the state “X 1 ” and an error covariance of each component in the state “X 2 ”.
  • An error covariance matrix “P 12 ” including the error covariance of each component in the state “X 1 ” for each component is also calculated.
  • the speed (hereinafter referred to as “GPS calculation speed”) calculated by the GPS unit 3A is used as the observation amount “Z”, and the state “X” predicted by the prediction calculation is used. to correct.
  • the correction calculation is performed using the observation amount “Z” given by the following equation (3).
  • (V E , V N , V U ) INS is the INS calculation speed calculated by the INS unit 2A in the ENU coordinate system.
  • (V X , V Y , V Z ) GPS ” is a GPS calculation speed calculated by the GPS unit 3A in the ECEF coordinate system.
  • C ECEF ENU is a coordinate transformation matrix from the ECEF coordinate system to the ENU coordinate system.
  • the first Kalman filter processing is configured such that a constraint condition based on the motion model of the moving object can be applied as the observation amount “Z” separately from the GPS calculation speed.
  • a constraint condition based on the motion model of the moving object can be applied as the observation amount “Z” separately from the GPS calculation speed.
  • speed constraints there are two types of speed constraints: a “speed restriction condition at stop” that is a speed restriction condition when the moving body is stopped and a “speed restriction condition at the time of movement” that is a speed restriction condition when the moving body is moving. Conditions are applicable.
  • the INS calculation speed is corrected using the speed error “( ⁇ V E , ⁇ V N , ⁇ V U )”, and the result is used as the first calculation speed.
  • the data is output to the second Kalman filter processing unit 7A.
  • the clock drift “d” is output to the second Kalman filter processor 7A as the first arithmetic clock drift.
  • the acceleration bias “(b ax , b ay , b az )” and the angular velocity bias “(b gx , b gy , b gz )” are fed back to the INS unit 2A.
  • the INS unit 2A uses the acceleration bias “(b ax , b ay , b az )” and the angular velocity bias “(b gx , b gy , b gz )” input from the first Kalman filter processing unit 5A to accelerate the acceleration. Compensate the sensor and gyro sensor.
  • the INS calculation position is corrected using the position error “( ⁇ P E , ⁇ P N , ⁇ P U )”, and the result is calculated as the first calculation. Output as position.
  • the first calculation position is used for various application processes as the final position of the moving body.
  • a predetermined error estimation calculation is performed to estimate a first calculation position error inherent in the first calculation position.
  • the first calculation position error “P1 err ” is calculated and estimated according to the following equation (4).
  • “ ⁇ t” is the calculation time interval of the first Kalman filter process.
  • the speed error “( ⁇ V E , ⁇ V N , ⁇ V U )” which is the first to third components of the state “X 1 ”
  • the position error in the calculation time interval can be reduced.
  • the first calculated position error “P1 err ” is calculated by adding the change to the position error “( ⁇ P E , ⁇ P N , ⁇ P U )” of the state “X 2 ”.
  • the second Kalman filter processing unit 7A uses the first calculation speed and the first calculation clock drift input from the first Kalman filter processing unit 5A as input U, and uses the GPS calculation position input from the GPS unit 3A as an observation amount.
  • the position of the moving object is calculated as “Z”.
  • the position calculated by the second Kalman filter processing unit 7A is output to the addition / subtraction unit 8 as the second calculation position.
  • the state “X” of the moving object to be estimated is set as in the following equation (5), for example.
  • the observation amount “Z” is set as in the following equation (6), for example.
  • the GPS calculation position is not the observation amount “Z”, and the GPS calculation speed is the observation amount “Z”. Instead, the GPS calculation position is the observation amount “Z” of the second Kalman filter process. This is because various error factors exist in GPS, and it is assumed that a large error may be included in the GPS calculation position.
  • a typical example is a multipath environment.
  • the position calculation is separated into a first calculation process (first Kalman filter process) and a second calculation process (second Kalman filter process), and the GPS calculation position is used in the first calculation process. Without moving, the position of the moving body was calculated.
  • GPS can calculate the speed of a moving object together with the position of the moving object.
  • the speed is less susceptible to multipath than the position, and does not significantly affect the coupling with the INS calculation result. Therefore, in the first Kalman filter process, the position of the moving object is calculated using the GPS calculation speed as an observation amount.
  • the addition / subtraction unit 8 calculates a difference between the second calculation position output from the second Kalman filter processing unit 7A and the INS calculation position output from the INS unit 2A. For convenience, this difference is referred to as a “second calculation position error”.
  • the second calculation position error “P2 err ” calculated by the addition / subtraction unit 8 is output to the Kalman filter coefficient adjustment unit 9A.
  • the Kalman filter coefficient adjustment unit 9A includes an INS calculation error estimation unit 91 and a filter coefficient calculation unit 93, and adjusts the filter coefficient related to the first Kalman filter processing.
  • the filter coefficient calculation unit 93 is illustrated and described as a functional unit of the first Kalman filter processing unit 5A.
  • the filter coefficient calculation unit 93 uses the INS calculation error estimated by the INS calculation error estimation unit 91 to calculate a filter coefficient related to the first Kalman filter process.
  • the filter coefficients to be adjusted are, for example, states “X 1 ”, “X 2 ” and error covariance matrices “P 11 ”, “P 22 ”, “P 21 ”, “P 12 ”. The method for adjusting the filter coefficient will be described later in detail.
  • the INS calculation error estimation unit 91 is a functional unit that estimates an INS calculation error that is an error included in the INS calculation result. For example, a position error weighted average unit 911, a weight setting unit 913, and an error conversion unit 915 are included. Have.
  • the position error weighted average unit 911 uses the weight set by the weight setting unit 913 to perform a weighted average of the first calculation position error “P1 err ” and the second calculation position error “P2 err ” to obtain the INS.
  • the calculation position error (inertial positioning error) of the unit 2A is calculated. Specifically, for example, the average position error “aveP err ” is calculated according to the following equation (7).
  • ⁇ is a weight for the first calculation position error “P1 err ”, and “0 ⁇ ⁇ ⁇ 1”.
  • the weight for the second calculation position error “P2 err ” is “1 ⁇ ”.
  • the weight setting unit 913 sets the weighted average weight “ ⁇ ” of the position error weighted average unit 911 based on a plurality of factors such as the positioning environment and the reliability of the measurement result of the GPS unit 3A.
  • the weight setting method will be described later in detail.
  • the error conversion unit 915 converts the average position error “aveP err ” input from the position error weighted average unit 911 into an INS calculation error using the error covariance “P” input from the first Kalman filter processing unit 5A. .
  • a method for converting the INS calculation error will also be described in detail later.
  • FIG. 3 is an explanatory diagram of input / output data of the first Kalman filter processing unit 5A.
  • a table of the correspondence relationship between the state “X”, the input “U”, and the observation amount “Z” is illustrated.
  • coupling There are various types of coupling. Among them, a method called loose coupling (loose coupling) and a method called tight coupling (tight coupling) are generally used.
  • the loose coupling method is a coupling method in which the connection between GPS and INS is relatively weak.
  • the state “X” is an INS calculation error (INS calculation position error, INS calculation speed error, INS calculation attitude angle error, INS acceleration bias, INS angular speed bias, clock bias, etc.).
  • the input “U” is the INS calculation result (INS calculation position, INS calculation speed, INS calculation attitude angle, etc.)
  • the observation amount “Z” is a constraint condition based on the GPS calculation result (GPS calculation speed, etc.) or the motion model ( Stop or move).
  • the tight coupling method is a coupling method in which the connection between GPS and INS is relatively strong.
  • the input “U” and the state “X” are the same as described above, and the observation amount “Z” is a constraint condition based on GPS measurement information (pseudo-range change rate, etc.) or a motion model.
  • Each component of the above state “X”, input “U”, and observation amount “Z” can be added / deleted as appropriate.
  • the bias component may be deleted, and the INS calculation position error, the INS calculation speed error, and the INS calculation posture angle error may be used as the component of the state “X”.
  • the observation amount ⁇ ⁇ “Z” the GPS calculation speed or pseudorange change rate and the constraint condition based on the motion model may be used in combination, or only one of them may be used.
  • weight “ ⁇ ” a high setting value “ ⁇ high ” defined as a relatively high value
  • a medium setting value “ ⁇ middle ” defined as a medium value
  • a low setting value defined as a relatively low value
  • One of the three values “ ⁇ low ” is set as the weight “ ⁇ ”. These specific values may be determined as appropriate.
  • FIG. 4 is a diagram illustrating an example of a first weight setting condition table in which first weight setting conditions relating to weight setting are defined.
  • a low weight setting condition and a high weight setting condition are defined in association with each other.
  • the low weight setting condition is a condition for setting the low setting value “ ⁇ low ” to the weight “ ⁇ ”.
  • the high weight setting condition is a condition for setting the high setting value “ ⁇ high ” to the weight “ ⁇ ”.
  • the middle setting value “ ⁇ middle ” is set to the weight “ ⁇ ”.
  • the first weight setting condition is a condition determined based on the positioning environment.
  • the low weight setting condition stipulates that the positioning environment is a multipath environment
  • the high weight setting condition stipulates that the positioning environment is an open sky environment.
  • the positioning environment is a multipath environment, an error occurs in the observed pseudorange due to the influence of indirect waves. Therefore, the accuracy of the position calculated by the GPS unit 3A is reduced.
  • the GPS calculation position is set to the observation amount “Z”. Therefore, if the accuracy of the GPS calculation position is low, the second calculation position also has an influence.
  • the second calculation position error “P2 err ” calculated as a difference between the second calculation position and the INS calculation position is a value including the influence of multipath. Therefore, when the positioning environment is a multi-path environment, the weight for the second operational position error “P2 err” to multipath effects emphasize second operational position error “P2 err” which is significantly reflected in Increase “1- ⁇ ”. This corresponds to reducing the weight “ ⁇ ” for the first calculation position error “P1 err ”.
  • FIG. 5 is a diagram illustrating an example of a second weight setting condition table in which second weight setting conditions relating to weight setting are defined.
  • a condition number that is a condition number, a low weight setting condition, and a high weight setting condition are defined in association with each other.
  • the reliability of the GPS calculation position is low, the accuracy of the second calculation position calculated by the second Kalman filter processing unit 7A decreases. Therefore, the influence is significantly reflected in the second calculation position error “P2 err ” calculated as a difference between the second calculation position and the INS calculation position. Therefore, when the reliability of the GPS calculation position is low, the weight “ ⁇ ” for the first calculation position error “P1 err ” is reduced to emphasize the second calculation position error “P2 err ”.
  • the reliability of the GPS calculation position can be determined by comprehensively considering a plurality of factors such as the reception environment and signal strength of the GPS satellite signal, the sky arrangement of the GPS satellite, and detection of a sudden change in position. For example, when the GPS satellites are not well positioned (for example, when the DOP (Dilution Of Precision) value is large) or when a large position jump occurs (a position far away from the true position can be obtained as a calculation result) When a rapid position change in the altitude direction occurs, it can be determined that the reliability of the GPS calculation position is low.
  • DOP Deution Of Precision
  • a second calculation position error “P2 err ” calculated as a difference between the second calculation position and the INS calculation position is calculated.
  • the weight “ ⁇ ” for the first calculation position error “P1 err ” is reduced.
  • the second calculation position changes discontinuously, or a sudden position jump occurs at the second calculation position. In this case, it is determined that the reliability of the second calculation position is low.
  • the condition “D” is a condition related to the reliability of time information.
  • time information reliability high
  • time information reliability low
  • the GPS time synchronization error exceeds a predetermined threshold (or more than the threshold), it is determined that the reliability of the time information is low.
  • the reliability of the determination item is quantified for each of a plurality of conditions. For example, the state with the lowest reliability of the judgment item is defined as reliability “0”, the state with the highest reliability is defined as reliability “1”, and the reliability of each judgment item is quantified in the range of “0 to 1” .
  • the weight setting index value “W” is calculated according to the following equation (8).
  • the first weight setting condition in FIG. 4 and the second weight setting condition in FIG. 5 may be used alone or in combination. That is, it is possible to set a weight based on the positioning environment and perform a weighted average, or set a weight based on the reliability of the measurement result of the GPS unit 3A and the like, and perform a weighted average. It is also possible to set a weight using these conditions in combination and perform a weighted average.
  • the weight “ ⁇ ” may be set using the determination result of the first weight setting condition and the determination result of the second weight setting condition as an AND condition or an OR condition.
  • the weight setting index value “W” of Expression (8) may be calculated including the quality of the measured positioning environment.
  • INS Calculation Error Estimation Method Next, an INS calculation error estimation method will be described.
  • a method for converting the average position error into the INS calculation error for example, a Kalman filter can be used.
  • “H 22 X 2 ” is a predicted value of the observation amount “Z 2 ” and corresponds to a predicted value of the average position error “aveP err ”. That is, this is an equation indicating that the average position error “aveP err ” is predicted by applying the observation matrix “H 22 ” to the state “X 2 ” given by Equation (2).
  • the observation matrix “H 22 ” is a conversion matrix from the state “X 2 ” to the observation amount “Z 2 ”.
  • Kalman gains “K 12 ” and “K 22 ” are calculated according to the following equations (10) and (11).
  • K 12 is a Kalman gain for calculating the state “X 1 ” from the difference “V 2 ”
  • K 22 is a Kalman gain for calculating the state “X 2 ” from the difference “V 2 ”.
  • R 2 is an observation error (observation noise) of the observation amount “Z 2 ”.
  • the first Kalman filter processing unit uses the velocity error and the posture angle error as the estimated velocity error “V err ” and the estimated posture angle error “A err ”. Feedback to 5A. Further, the position error that is a component of the state “X 2 ” calculated according to the equation (13) is fed back to the first Kalman filter processing unit 5A as the estimated position error “P err ”. The Kalman gains “K 12 ” and “K 22 ” are fed back to the first Kalman filter processing unit 5A.
  • the filter coefficient calculation unit 93 receives the INS calculation error (estimated position error “P err ”, estimated speed error “V err ” and estimated attitude angle error “A err ”) input from the INS calculation error estimation unit 91 and the Kalman gain “K”. The filter coefficient is calculated using “ 12 ” and “K 22 ”.
  • the filter coefficient calculation unit 93 converts the estimated speed error “V err ” and the estimated attitude angle error “A err ” input from the INS calculation error estimation unit 91 into the corresponding component of the state “X 1 ” (
  • the state “X 1 ” is calculated and updated in consideration of the speed error “( ⁇ V E , ⁇ V N , ⁇ V U )” and the attitude angle error “( ⁇ x , ⁇ y , ⁇ z )”).
  • the estimated position error “P err ” input from the INS calculation error estimator 91 is added to each component (position error “( ⁇ P E , ⁇ P N , ⁇ P U )”) of the state “X 2 ”, and the state “ X 2 ′′ is calculated / updated.
  • the filter coefficient calculation unit 93 uses the Kalman gains “K 12 ” and “K 22 ”, for example, according to the following equations (14) to (17), the error covariance matrices “P 11 ”, “P 22 ”. , “P 21 ” and “P 12 ” are calculated and updated.
  • FIGS. 6 and 7 are diagrams illustrating an example of experimental results obtained by performing position calculation using the position calculation method described above.
  • the first position calculation device 1A was installed in an automobile, and an experiment was performed in which an automobile travels on an urban road.
  • the urban area where we traveled is a multipath environment with many high-rise buildings.
  • the position was calculated by circling the urban road in FIGS. 6 and 7 counterclockwise, and the locus was drawn. The start and goal points are at the bottom center of the drawing.
  • FIG. 6 (1) is a true trajectory (reference trajectory) of the car.
  • FIG. 6B is a locus of a GPS calculation position that is an output of the GPS unit 3A.
  • FIG. 6 (3) is a locus of the second calculation position, which is the output of the second Kalman filter processing unit 7A.
  • FIG. 7 (1) shows the locus of the first calculation position (no calculation coefficient adjustment), which is the output of the first Kalman filter processing unit 5A when the calculation coefficient is not adjusted.
  • FIG. 7 (2) shows the locus of the first calculation position (with calculation coefficient adjustment and no weight setting) when the calculation coefficient is adjusted but the weight “ ⁇ ” is not set.
  • FIG. 7 (3) shows the locus of the first calculation position (with calculation coefficient adjustment and with weight setting) when both adjustment of the calculation coefficient and setting of the weight “ ⁇ ” are performed.
  • the trajectory of the second computation position in FIG. 6 (3) shows a smooth trajectory compared to the trajectory of the GPS computation position, but still, the trajectory along the true trajectory in FIG. 6 (1). It is not.
  • FIG. 7 (3) If the setting of the weight “ ⁇ ” of this embodiment is further added to this, the result of FIG. 7 (3) is obtained. Looking at the locus of the first calculation position in FIG. 7 (3) (with calculation coefficient adjustment and weight setting), it can be seen that an accurate locus along the true locus in FIG. 6 (1) is obtained. Recognize. In particular, the position calculation accuracy is also improved for the portion corresponding to the dotted line portion S1 in FIG. 7A and the dotted line portion S2 in FIG. From this experimental result, it was proved that the position calculation method of this embodiment is effective.
  • FIG. 8 is an explanatory diagram of the system configuration of the navigation system 1000 in the present embodiment.
  • the navigation system 1000 is configured such that a car navigation device 100 including the position calculation device 1 is installed in a four-wheeled vehicle (hereinafter simply referred to as “automobile”) that is a kind of mobile body.
  • automobile a four-wheeled vehicle
  • the car navigation device 100 is an electronic device that is installed in a car and performs navigation for the driver of the car.
  • the car navigation apparatus 100 includes an INS unit 2A and a GPS unit 3A.
  • the car navigation apparatus 100 calculates the position of the vehicle according to the position calculation method described in the principle, using the INS calculation result input from the INS unit 2A and the GPS calculation result input from the GPS unit 3A. Then, a navigation screen in which the calculated positions are plotted is generated and displayed on the display that is the display unit 30.
  • FIG. 9 is a block diagram illustrating an example of a functional configuration of the car navigation device 100.
  • the car navigation device 100 includes an INS unit 2A, a GPS unit 3A, a processing unit 10, an operation unit 20, a display unit 30, a communication unit 40, and a storage unit 50.
  • the processing unit 10 is a control device that comprehensively controls each unit of the car navigation apparatus 100 according to various programs such as a system program stored in the storage unit 50, and includes a processor such as a CPU (Central Processing Unit). Composed.
  • the processing unit 10 performs navigation processing according to the navigation program 51 stored in the storage unit 50 and causes the display unit 30 to display a map indicating the current position of the automobile.
  • the operation unit 20 is an input device configured by, for example, a touch panel or a button switch, and outputs a signal of a pressed key or button to the processing unit 10. By operating the operation unit 20, various instructions such as destination input are input.
  • the display unit 30 is configured by an LCD (Liquid Crystal Display) or the like, and is a display device that performs various displays based on a display signal input from the processing unit 10. A navigation screen or the like is displayed on the display unit 30.
  • LCD Liquid Crystal Display
  • the communication unit 40 is a communication device for exchanging information used inside the device with the outside via a communication network such as the Internet under the control of the processing unit 10.
  • a communication network such as the Internet under the control of the processing unit 10.
  • a known wireless communication technique can be applied to this communication.
  • the storage unit 50 is configured by a storage device such as a ROM (Read Only Memory), a flash ROM, or a RAM (Random Access Memory), and various types for realizing various functions such as a system program of the car navigation device 100 and a navigation function. Stores programs, data, etc. In addition, it has a work area for temporarily storing data being processed and results of various processes.
  • a storage device such as a ROM (Read Only Memory), a flash ROM, or a RAM (Random Access Memory)
  • various types for realizing various functions such as a system program of the car navigation device 100 and a navigation function. Stores programs, data, etc. In addition, it has a work area for temporarily storing data being processed and results of various processes.
  • the storage unit 50 stores a navigation program 51 that is read as a program by the processing unit 10 and executed as a navigation process (see FIG. 10).
  • the navigation program 51 includes an INS calculation error estimation program 511 executed as an INS calculation error estimation process (see FIG. 11) as a subroutine.
  • a weight setting condition table 53 As data, for example, a weight setting condition table 53, an INS calculation result 55, a GPS calculation result 56, a first calculation result 57, a second calculation result 58, and a calculation coefficient are stored. 59 is stored.
  • the weight setting condition table 53 is a table in which conditions for weight setting such as a first weight setting condition table (see FIG. 4) and a second weight setting condition table (see FIG. 5) are defined.
  • FIG. 10 is a flowchart showing the flow of navigation processing executed by the processing unit 10 according to the navigation program 51 stored in the storage unit 50.
  • the processing unit 10 starts acquiring the INS calculation result 55 and the GPS calculation result 56 from the INS unit 2A and the GPS unit 3A (step A1). Then, the processing unit 10 executes the first Kalman filter process (step A3).
  • the filter coefficient is adjusted using the estimation result of the previous INS calculation error estimation process (step A9), and the calculation coefficient 59 of the storage unit 50 is updated. Specifically, as described in the principle, the state “X” is adjusted using the estimated INS calculation error, and the error covariance “P” is adjusted according to, for example, the equations (14) to (17). Then, Kalman filter processing is performed using the adjusted filter coefficient, and the first calculation result 57 of the storage unit 50 is updated with the calculation result.
  • the processing unit 10 performs a map matching process on the calculation position calculated by the first Kalman filter process, and updates and displays the navigation screen of the display unit 30 as a result (step A5).
  • the processing unit 10 executes a second Kalman filter process (step A7).
  • the processing unit 10 updates the second calculation result 58 of the storage unit 50 with the calculation result of the second Kalman filter process.
  • the processing unit 10 performs INS calculation error estimation processing according to the INS calculation error estimation program 511 stored in the storage unit 50 (step A9).
  • FIG. 11 is a flowchart showing the flow of the INS calculation error estimation process.
  • the processing unit 10 performs weight setting processing (step B1). Specifically, the weight “ ⁇ ” is set according to the method described in the principle with reference to the weight setting condition table 53 stored in the storage unit 50.
  • the processing unit 10 performs a position error weighted average process (step B3). Specifically, the first calculation position error “P1 err ” calculated by the first Kalman filter process and the second calculation position error “P2 err ” calculated by the second Kalman filter process are set in step B1.
  • the average position error “aveP err ” is calculated by performing the weighted average according to the equation (7) using the weight “ ⁇ ” set in (1).
  • step B5 the processing unit 10 performs error conversion processing. Specifically, the average position error “aveP err ” calculated in step B3 is converted into an INS calculation error according to equations (9) to (13). Then, the processing unit 10 ends the INS calculation error estimation process.
  • the processing unit 10 determines whether or not to end the process (step A11). For example, when a navigation end instruction operation is performed by the user via the operation unit 20, it is determined that the navigation process is to be ended.
  • step A11; No If it is determined that the process is not terminated (step A11; No), the processing unit 10 returns to step A3. If it is determined that the process is to be terminated (step A11; Yes), the navigation process is terminated.
  • the first calculation process for calculating at least the position of the moving body using the measurement result of the inertial positioning unit 2 installed in the moving body is the first. It is executed by the arithmetic processing unit 5.
  • the second calculation processing unit 7 calculates the position of the moving object using the result of the first calculation process and the measurement result of the satellite positioning unit 3 installed in the moving object. It is executed by. Then, using the result of the first calculation process and the result of the second calculation process, the calculation coefficient related to the first calculation process is adjusted by the calculation coefficient adjustment unit 9.
  • the first Kalman filter processing unit 5A receives the calculation result (INS calculation result) of the INS unit as an input, and the calculation speed (GPS A Kalman filter process using the observation speed as the calculation speed is executed. Further, in the second Kalman filter processing unit 7A, the calculation result (first calculation result) of the first Kalman filter processing unit 5A is input, and the calculation position (GPS calculation position) of the GPS unit 3A is the observation amount. Kalman filtering is performed.
  • the addition / subtraction unit 8 calculates the difference between the INS calculation position and the second calculation position as a second calculation position error.
  • the weight setting unit 913 sets a weight for weighted averaging of the first calculation position error and the second calculation position error. Then, the first calculation position error and the second calculation position error are weighted and averaged by the position error weighted average unit 911. Then, using the average position error that is the weighted average result, the error conversion unit 915 converts the average position error into an INS calculation error, and the conversion result is fed back to the first Kalman filter processing unit 5A.
  • the weight setting unit 913 includes, for example, the first weight setting condition determined based on the positioning environment, the reliability of the GPS calculation position, the reliability of the GPS measurement information, the reliability of the second calculation position, and the reliability of the time information.
  • a weighted average weight is set according to a second weight setting condition determined based on a plurality of factors such as sex. Thereby, the weight of the weighted average can be optimized, and the error included in the INS calculation position can be estimated appropriately.
  • the error conversion unit 915 performs a conversion operation using the Kalman filter by using the error covariance input from the first Kalman filter processing unit 5A and the average position error input from the position error weighted average unit 911. Then, the INS calculation error is estimated. By using the Kalman filter, the INS calculation error can be appropriately estimated from the average position error.
  • the filter coefficient calculation unit 93 calculates and adjusts the filter coefficient used in the first Kalman filter processing, using the INS calculation error and the Kalman gain estimated by the INS calculation error estimation unit 91. For example, the state (state vector) and error covariance (error covariance matrix) used in the first Kalman filter process are calculated and adjusted. By performing the first Kalman filter process using the filter coefficient adjusted in this way, the position of the moving body can be obtained with high accuracy.
  • the GPS unit 3A to which GPS is applied has been described as an example of the satellite positioning unit 3.
  • a WAAS Wide Area Augmentation System
  • QZSS Quadrati Zenith Satellite System
  • GLONASS GLObal NAvigation
  • it may be a unit to which another satellite positioning system such as Satellite System) or GALILEO is applied.
  • the case where the INS unit 2A is applied as the inertial positioning unit 2 has been described as an example.
  • the INS measurement information measured by the inertial positioning unit 2 may be used as the input U so that the first arithmetic processing unit 5 performs the first arithmetic processing.
  • the arithmetic processing performed by the first arithmetic processing unit 5 and the second arithmetic processing unit 7 is not limited to Kalman filter processing.
  • sigma point filter processing using sigma points (sigma points) or regression filter processing may be applied to the second calculation processing.
  • the INS unit 2A has been described as calculating the position, velocity, and attitude angle of the moving body in the ENU coordinate system.
  • the INS unit 2A may calculate in the NED coordinate system or the ECEF coordinate system.
  • the first calculation process and the second calculation process may be calculated using the NED coordinate system or the ECEF coordinate system instead of using the ENU coordinate system.
  • Weight Setting Conditions The weight setting conditions exemplified in the above embodiment are merely examples, and can be added / deleted as appropriate. For example, a condition based on the reliability of the calculation result of the INS unit 2A may be determined. If a large error is mixed in the measurement result of the inertial sensor (gyro sensor or acceleration sensor) included in the INS unit 2A, the calculation accuracy of the INS unit 2A decreases. In this case, the calculation accuracy of the first calculation process using the INS calculation result as input decreases.
  • the INS unit 2A performs the INS calculation
  • the GPS unit 3A performs the GPS calculation
  • the processing unit 10 of the electronic device may be configured to execute INS calculation or GPS calculation.
  • the INS unit 2A outputs INS measurement information (acceleration, angular velocity, etc.).
  • the GPS unit 3A outputs GPS measurement information (code phase, Doppler frequency, pseudo distance, pseudo distance change rate, etc.).
  • the processing unit 10 of the electronic device performs the INS calculation using the INS measurement information input from the INS unit 2A, and performs the GPS calculation using the GPS measurement information input from the GPS unit 3A. Then, the processing unit 10 performs the first and second calculation processes using the INS calculation result and the GPS calculation result.
  • the present invention can be similarly applied to electronic devices other than navigation.
  • the present invention can be similarly applied to other electronic devices such as a mobile phone, a personal computer, and a PDA (Personal Digital Assistant), and position calculation of the electronic device can be realized.
  • PDA Personal Digital Assistant
  • 1, 1A position calculation device 1, 1A position calculation device, 2 inertial positioning unit, 2A INS unit, 3 satellite positioning unit, 3A GPS unit, 5 1st arithmetic processing unit, 5A 1st Kalman filter processing unit, 7 2nd arithmetic processing Unit, 7A, second Kalman filter processing unit, 9 calculation coefficient adjustment unit, 9A Kalman filter coefficient adjustment unit, 10 processing unit, 20 operation unit, 30 display unit, 40 communication unit, 50 storage unit, 100 car navigation device, 1000 Navigation system.

Abstract

　慣性測位用ユニット及び衛星測位用ユニットそれぞれの計測結果を併用して、位置をより正確に算出するための手法の提案。　位置算出装置（１）において、移動体に設置された慣性測位用ユニット（２）の計測結果を用いて、少なくとも移動体の位置を演算する第１の演算処理が第１の演算処理部（５）により実行される。また、第１の演算処理の結果と、移動体に設置された衛星測位用ユニット（３）の計測結果とを用いて、移動体の位置を演算する第２の演算処理が第２の演算処理部（７）により実行される。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数が演算係数調整部（９）により調整される。

Description

位置算出方法及び位置算出装置
　本発明は、慣性測位用ユニット及び衛星測位用ユニットそれぞれの計測結果を併用した位置算出方法等に関する。
　いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、慣性センサーの活用が注目されている。慣性センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。慣性センサーの検出結果を利用して慣性航法演算を行う慣性航法システム（以下、「ＩＮＳ（Inertial Navigation System）」と称する。）も考案されている。
　ＩＮＳでは、慣性センサーの検出結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献１には、ＩＮＳ演算結果を、ＧＰＳ（Global Positioning System）を利用して補正する技術が開示されている。
米国特許出願公開第２０１０／００１９９６３号明細書
　ＧＰＳを利用してＩＮＳ演算結果を補正する技術は、ＧＰＳ演算結果が正しいことを前提としている。特許文献１に開示された技術も同様である。しかし、ＧＰＳ演算結果は、ＧＰＳ衛星から受信したＧＰＳ衛星信号の信号強度、受信環境、ＧＰＳ衛星の天空配置、マルチパスといった種々の要因により、演算結果の精度が低下する場合がある。
　ＧＰＳ演算結果の精度が低下しているにも関わらず、特許文献１の技術のようにＧＰＳ演算結果を用いてＩＮＳ演算結果を補正してしまうと、位置算出の正確性が低下するという問題があった。
　本発明は上述した課題に鑑みて為されたものであり、衛星測位用ユニット及び慣性測位用ユニットそれぞれの計測結果を併用して、位置をより正確に算出するための手法を提案することを目的とする。
　以上の課題を解決するための第１の形態は、移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行することと、前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行することと、前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整することと、を含む位置算出方法である。
　また、他の形態として、移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行する第１の演算処理部と、前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行する第２の演算処理部と、前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整する調整部と、を備えた位置算出装置を構成してもよい。
　この第１の形態等によれば、移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも移動体の位置を演算する第１の演算処理を実行する。また、第１の演算処理の結果と、移動体に設置された衛星測位用ユニットの計測結果とを用いて、移動体の位置を演算する第２の演算処理を実行する。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数を調整する。
　演算処理には、慣性測位用ユニットの計測結果を用いた第１の演算処理と、第１の演算処理の結果及び衛星測位用ユニットの計測結果を用いた第２の演算処理とがある。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数を調整することで、第１の演算処理における位置算出の正確性を向上させることができる。
　また、第２の形態として、第１の形態の位置算出方法において、前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、前記第１の演算処理は、演算した第１の演算位置に内在する第１の位置誤差を推定する所定の誤差推定演算を含み、前記調整することは、前記第１の位置誤差と、前記第２の演算処理で求めた第２の演算位置とを用いて、前記慣性測位位置に内在する慣性測位誤差を推定することと、前記慣性測位誤差を用いて前記演算係数を調整することと、を含む、位置算出方法を構成することとしてもよい。
　この第２の形態によれば、所定の誤差推定演算を行って、第１の演算処理で演算した第１の演算位置に内在する第１の位置誤差を推定する。そして、第１の位置誤差と、第２の演算処理で求めた第２の演算位置とを用いて、慣性測位用ユニットの計測結果に含まれる慣性測位位置に内在する慣性測位誤差を推定する。第１の位置誤差と併せて第２の演算位置を用いることで、慣性測位誤差を適切に推定することができる。そして、当該慣性測位誤差を用いて第１の演算処理に係る演算係数を調整することで、第１の演算処理における位置算出の正確性を向上させることができる。
　また、第３の形態として、第２の形態の位置算出方法において、前記推定することは、前記慣性測位位置と前記第２の演算位置との差分を算出することと、前記第１の位置誤差と前記差分とを平均処理して前記慣性測位誤差を算出することと、を含む、位置算出方法を構成することとしてもよい。
　この第３の形態によれば、慣性測位位置と第２の演算位置との差分を算出する。そして、第１の位置誤差と当該差分とを平均処理することで、慣性測位誤差を適切に推定することができる。
　また、第４の形態として、第３の形態の位置算出方法において、前記平均処理は、測位環境と前記衛星測位用ユニットの計測結果の信頼性とのどちらか又は両方に基づいて、前記第１の位置誤差と前記差分との重みを設定して加重平均する処理である、位置算出方法を構成することとしてもよい。
　この第４の形態によれば、平均処理において、測位環境と衛星測位用ユニットの計測結果の信頼性とのどちらか又は両方に基づいて、第１の位置誤差と差分との重みを設定して加重平均する。これにより、測位環境や衛星測位用ユニットの計測結果の信頼性に基づいて、慣性測位誤差をより正確に推定することができる。
　また、第５の形態として、第１～第４の何れかの形態の位置算出方法において、前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、前記第１の演算処理は、前記慣性測位位置を入力とするカルマンフィルター処理である、位置算出方法を構成することとしてもよい。
　この第５の形態によれば、第１の演算処理として慣性測位位置を入力とするカルマンフィルター処理を行うことで、移動体の位置等を簡易且つ適切に求めることができる。
　また、第６の形態として、第５の形態の位置算出方法において、前記衛星測位用ユニットの計測結果には速度が含まれ、前記カルマンフィルター処理は、前記速度を観測量とする処理である、位置算出方法を構成してもよい。
　この第６の形態によれば、第５の形態のカルマンフィルター処理において、さらに衛星測位用ユニットの計測結果に含まれる速度を観測量とすることで、移動体の位置等をより正確に求めることができる。
　また、第７の形態として、第１～第６の何れかの形態の位置算出方法において、前記衛星測位用ユニットの計測結果には衛星測位位置が含まれ、前記第２の演算処理は、前記第１の演算処理の結果を入力とし、前記衛星測位位置を観測量とするカルマンフィルター処理である、位置算出方法を構成することとしてもよい。
　この第７の形態によれば、第２の演算処理として、第１の演算処理の結果を入力とし、衛星測位用ユニットの計測結果に含まれる衛星測位位置を観測量とするカルマンフィルター処理を実行することで、移動体の位置を簡易且つ適切に求めることができる。
位置算出装置の構成図。 第１の位置算出装置の構成図。 第１のカルマンフィルター処理の入出力データの説明図。 第１の重み設定条件の説明図。 第２の重み設定条件の説明図。 位置算出を行った実験結果の一例を示す図。 位置算出を行った実験結果の一例を示す図。 ナビゲーションシステムのシステム構成図。 カーナビゲーション装置の機能構成を示すブロック図。 ナビゲーション処理の流れを示すフローチャート。 ＩＮＳ演算誤差推定処理の流れを示すフローチャート。
　１．原理
　１－１．構成
　図１は、本実施形態における位置算出装置１の主要構成図である。位置算出装置１は、移動体に備えられ、当該移動体の位置を算出する装置（システム）である。移動体は、自動車やオートバイ、自転車、船、電車といった物の他、人間自体であってもよい。人間が位置算出装置１を携帯し、人間自身が位置算出装置１を備えることとしてもよい。
　本明細書で参照する図面では、ユニット（モジュール）を二重線で図示し、ユニットの計測結果を利用して演算処理を行う処理ブロックを一重線で図示することで、両者を区別する。
　位置算出装置１には、ユニット（モジュール）として、慣性測位用ユニット２と、衛星測位用ユニット３とが含まれる。また、位置算出装置１は、主要な処理ブロックとして、第１の演算処理部５と、第２の演算処理部７と、演算係数調整部９とを有する。なお、ユニットと処理ブロックとで位置算出装置１を構成してもよいし、ユニットを除外して処理ブロックのみで位置算出装置１を構成してもよい。
　慣性測位用ユニット２は、慣性航法を利用して測位を行うためのユニットである。慣性測位用ユニット２は、加速度センサーやジャイロセンサー等の慣性センサーや、慣性センサーをパッケージ化した慣性計測ユニット（ＩＭＵ（Inertial Measurement Unit））、慣性計測ユニット及び演算処理部をパッケージ化した慣性航法システム（ＩＮＳ（Inertial Navigation System））等を適用したユニットである。
　衛星測位用ユニット３は、衛星測位システムを利用して測位を行うためのユニットであり、例えば衛星測位システムの一種であるＧＰＳ（Global Positioning System）を適用可能である。
　第１の演算処理部５は、慣性測位用ユニット２の計測結果と、所与の観測量とを併用して、予め定められた第１の演算処理を行って、少なくとも移動体の位置を算出する。本実施形態において、第１の演算処理に好適な適用例は、カルマンフィルター処理である。
　第２の演算処理部７は、第１の演算処理部５の演算結果である第１の演算結果と、衛星測位用ユニット３の計測結果とを用いて、予め定められた第２の演算処理を行って、移動体の位置を算出する。本実施形態において、第２の演算処理に好適な適用例は、カルマンフィルター処理やシグマポイントフィルター処理、回帰フィルター処理である。
　演算係数調整部９は、第１の演算処理部５から入力した第１の演算結果と、第２の演算処理部７から入力した第２の演算結果とを用いて、第１の演算処理に係る演算係数を調整する。第１の演算処理部５は、演算係数調整部９により調整された演算係数を用いて第１の演算処理を行う。
　図２は、図１の位置算出装置１を適用した第１の位置算出装置１Ａの構成図である。第１の位置算出装置１Ａは、ＩＮＳユニット２Ａと、ＧＰＳユニット３Ａと、第１のカルマンフィルター処理部５Ａと、第２のカルマンフィルター処理部７Ａと、カルマンフィルター係数調整部９Ａとを有する。
　第１の位置算出装置１Ａは、慣性測位用ユニット２としてＩＮＳユニット２Ａを適用し、衛星測位用ユニット３としてＧＰＳユニット３Ａを適用したシステムである。また、第１及び第２の演算処理部５，７として、第１及び第２のカルマンフィルター処理部５Ａ，７Ａをそれぞれ適用したシステムである。
　ＩＮＳユニット２Ａは、ＩＭＵがローカル座標系で計測した加速度や角速度等のＩＮＳメジャメント情報を出力可能に構成されている。また、ＩＮＳユニット２Ａは、ＩＮＳメジャメント情報を用いた慣性航法演算を行って、移動体の位置や速度、姿勢角を演算して出力可能に構成されている。本実施形態では、ＩＮＳユニット２Ａは、ＩＮＳ演算位置（慣性測位位置）、ＩＮＳ演算速度及びＩＮＳ演算姿勢角を絶対座標系で演算して出力する。
　ＧＰＳユニット３Ａは、ＧＰＳ衛星から発信されているＧＰＳ衛星信号を受信して、コード位相やドップラー周波数、擬似距離、擬似距離変化率等のＧＰＳメジャメント情報を計測して出力可能に構成されている。また、ＧＰＳユニット３Ａは、ＧＰＳメジャメント情報を用いたＧＰＳ演算を行って、移動体の位置や速度を演算して出力可能に構成されている。本実施形態では、ＧＰＳユニット３Ａは、ＧＰＳ演算位置（衛星測位位置）及びＧＰＳ演算速度を絶対座標系で演算して出力する。
　ローカル座標系は、ＩＮＳユニット２Ａが有する慣性センサーに対応付けられたローカルな座標系（センサー座標系）である。それに対し、絶対座標系は、移動体の移動空間を定める座標系である。例えば、北東下座標系として知られるＮＥＤ（North East Down）座標系や、東北上座標系として知られるＥＮＵ（East North Up）座標系、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系といった座標系を適用可能である。
　以下の数式では、ＩＮＳユニット２Ａは、移動体の位置、速度及び姿勢角をＥＮＵ座標系で演算して出力し、ＧＰＳユニット３Ａは、移動体の位置及び速度をＥＣＥＦ座標系で演算して出力するものとして説明する。また、第１及び第２のカルマンフィルター処理では、ＥＮＵ座標系で演算するものとして説明する。また、加速度や速度といった諸量は、実際には大きさ及び方向を持ったベクトルとして表される。しかし、本明細書では、ベクトルの文言を省略して、簡単に加速度や速度として説明する。
　第１のカルマンフィルター処理部５Ａは、カルマンフィルターの理論に基づく演算処理を行って、移動体の位置や速度、姿勢角等を演算する。例えば、ＩＮＳユニット２Ａから入力したＩＮＳ演算位置、ＩＮＳ演算速度及びＩＮＳ演算姿勢角を入力Ｕ（制御入力）とする。また、ＧＰＳユニット３Ａから入力したＧＰＳ演算速度や、移動体の運動モデルに基づき定められる所与の制約条件を観測量Ｚとする。そして、カルマンフィルターの予測演算（時刻更新）及び補正演算（観測更新）を行って、状態推定値を求める。
　第１のカルマンフィルター処理では、推定対象とする移動体の状態“Ｘ”を、例えば次式（１）及び（２）のように設定する。
Figure JPOXMLDOC01-appb-M000001
Figure JPOXMLDOC01-appb-M000002
　式（１）で表される状態“Ｘ1”において、“（δＶE，δＶN，δＶU）”は、ＥＮＵ座標系で演算したＩＮＳ演算速度の誤差である。“（Ψx，Ψy，Ψz）”は、ローカル座標系で演算したＩＮＳ演算姿勢角の誤差である。“（ｂax，ｂay，ｂaz）”は、ローカル座標系で計測した加速度のバイアスである。“（ｂgx，ｂgy，ｂgz）”は、ローカル座標系で計測した角速度のバイアスである。また、“ｄ”は、ＧＰＳユニット３Ａの内部クロックのドリフト（クロックドリフト）である。
　また、式（２）で表される状態“Ｘ2”において、“（δＰE，δＰN，δＰU）”は、ＥＮＵ座標系で演算したＩＮＳ演算位置の誤差である。
　式（１）及び（２）から分かるように、本実施形態における第１のカルマンフィルター処理は、ＩＮＳユニット２Ａの演算結果に含まれる誤差を推定する誤差推定型のカルマンフィルター処理である。つまり、ＩＮＳユニット２Ａの演算結果に含まれる誤差を、状態 “Ｘ1”及び“Ｘ2”として推定する。
　また、第１のカルマンフィルター処理では、状態“Ｘ1”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ11”と、状態“Ｘ2”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ22”と、状態“Ｘ1”の各成分に対する状態“Ｘ2”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ21”と、状態“Ｘ2”の各成分に対する状態“Ｘ1”の各成分の誤差の共分散を含む誤差共分散行列“Ｐ12”と、を併せて演算する。
　第１のカルマンフィルター処理の補正演算では、ＧＰＳユニット３Ａにより演算された速度（以下、「ＧＰＳ演算速度」と称す。）を観測量“Ｚ”として、予測演算で予測された状態“Ｘ”を補正する。
　具体的には、例えば次式（３）で与えられる観測量“Ｚ”を用いて補正演算を行う。
Figure JPOXMLDOC01-appb-M000003
 　但し、“（ＶE，ＶN，ＶU）INS”は、ＩＮＳユニット２ＡがＥＮＵ座標系で演算したＩＮＳ演算速度である。また、“（ＶX，ＶY，ＶZ）GPS”は、ＧＰＳユニット３ＡがＥＣＥＦ座標系で演算したＧＰＳ演算速度である。また、“ＣECEF ENU”は、ＥＣＥＦ座標系からＥＮＵ座標系への座標変換行列である。
　さらに、第１のカルマンフィルター処理では、ＧＰＳ演算速度とは別に、移動体の運動モデルに基づく制約条件を観測量“Ｚ”として適用可能に構成されている。具体的には、移動体の停止時における速度制約条件である「停止時速度制約条件」と、移動体の移動時における速度制約条件である「移動時速度制約条件」との２種類の速度制約条件を適用可能である。
　停止時速度制約条件（第１の制約条件）は、移動体の停止時に適用可能な制約条件である。移動体が停止しているのであれば、理想的には移動体の速度はゼロである。従って、移動体が停止していると判定した場合は「移動体の各軸の速度成分＝０」を観測量“Ｚ”として与えることができる。
　移動時速度制約条件（第２の制約条件）は、移動体の移動時に適用可能な制約条件である。例えば、移動体として四輪自動車を想定した場合、通常、四輪自動車はジャンプや横滑りすることはないと仮定することができる。従って、移動体が移動していると判定した場合は「移動体の縦横方向の速度成分＝０」を観測量“Ｚ”として与えることができる。四輪自動車以外の移動体についても、当該移動体の移動方向や速度の大きさに関する制約に基づいて、移動時制約条件を適宜設定可能である。
　第１のカルマンフィルター処理で求めた状態“Ｘ1”について、その速度誤差“（δＶE，δＶN，δＶU）”を用いてＩＮＳ演算速度を補正し、その結果を第１の演算速度として第２のカルマンフィルター処理部７Ａに出力する。クロックドリフト“ｄ”は、第１の演算クロックドリフトとして第２のカルマンフィルター処理部７Ａに出力する。
　また、加速度バイアス“（ｂax，ｂay，ｂaz）”及び角速度バイアス“（ｂgx，ｂgy，ｂgz）”をＩＮＳユニット２Ａにフィードバックする。ＩＮＳユニット２Ａは、第１のカルマンフィルター処理部５Ａから入力した加速度バイアス“（ｂax，ｂay，ｂaz）”及び角速度バイアス“（ｂgx，ｂgy，ｂgz）”を用いて、加速度センサー及びジャイロセンサーを補償（キャリブレーション）する。
　また、第１のカルマンフィルター処理で求めた状態“Ｘ2”について、その位置誤差“（δＰE，δＰN，δＰU）”を用いてＩＮＳ演算位置を補正し、その結果を第１の演算位置として出力する。第１の演算位置は、最終的な移動体の位置として、各種のアプリケーション処理に利用される。
　また、第１のカルマンフィルター処理では、所定の誤差推定演算を行って、第１の演算位置に内在する第１の演算位置誤差を推定する。具体的には、例えば次式（４）に従って第１の演算位置誤差“Ｐ１err”を算出・推定する。
Figure JPOXMLDOC01-appb-M000004
　式（４）において、“Δｔ”は、第１のカルマンフィルター処理の演算時間間隔である。状態“Ｘ1”の第１～第３成分である速度誤差“（δＶE，δＶN，δＶU）”に演算時間間隔“Δｔ”を乗算することで、その演算時間間隔での位置誤差の変化分を求める。そして、当該変化分を状態“Ｘ2”の位置誤差“（δＰE，δＰN，δＰU）”に加算することで、第１の演算位置誤差“Ｐ１err”を算出する。
　第２のカルマンフィルター処理部７Ａは、第１のカルマンフィルター処理部５Ａから入力した第１の演算速度及び第１の演算クロックドリフトを入力Ｕとし、ＧＰＳユニット３Ａから入力したＧＰＳ演算位置を観測量“Ｚ”として、移動体の位置を演算する。第２のカルマンフィルター処理部７Ａにより演算された位置は、第２の演算位置として加減算部８に出力される。
　第２のカルマンフィルター処理では、推定対象とする移動体の状態“Ｘ”を、例えば次式（５）のように設定する。
Figure JPOXMLDOC01-appb-M000005
 　但し、“（ＰE，ＰN，ＰU）”は、ＥＮＵ座標系で表した移動体の位置である。“ｄ”はクロックドリフトである。
　また、第２のカルマンフィルター処理では、観測量“Ｚ”を、例えば次式（６）のように設定する。
Figure JPOXMLDOC01-appb-M000006
 　但し、“（ＰX，ＰY，ＰZ）GPS”は、ＥＣＥＦ座標系で表したＧＰＳ演算位置である。
　本実施形態において特徴的であるのは、第１のカルマンフィルター処理では、ＧＰＳ演算位置を観測量“Ｚ”とせず、ＧＰＳ演算速度を観測量“Ｚ”としている点である。その代わり、ＧＰＳ演算位置は、第２のカルマンフィルター処理の観測量“Ｚ”とする。これは、ＧＰＳでは種々の誤差要因が存在するため、ＧＰＳ演算位置に大きな誤差が含まれ得ることを想定したためである。その典型例はマルチパス環境である。
　マルチパス環境でＧＰＳ演算を行うと、演算される位置の正確性が低下する。そのため、ＧＰＳ演算位置とＩＮＳ演算位置とを単純にカップリングしてしまうと、ＧＰＳ演算位置の誤差に引きずられて、演算位置の正確性が低下するおそれがある。そこで、位置演算を、第１の演算処理（第１のカルマンフィルター処理）と第２の演算処理（第２のカルマンフィルター処理）とに分離し、第１の演算処理では、ＧＰＳ演算位置を用いずに移動体の位置を演算することとした。
　また、ＧＰＳでは、移動体の位置と併せて、移動体の速度も演算することができる。速度は位置と比べてマルチパスの影響を受けにくく、ＩＮＳ演算結果とのカップリングにそれほど大きな影響を与えない。そこで、第１のカルマンフィルター処理では、ＧＰＳ演算速度を観測量として用いて移動体の位置を演算することとした。
　加減算部８は、第２のカルマンフィルター処理部７Ａから出力された第２の演算位置と、ＩＮＳユニット２Ａから出力されたＩＮＳ演算位置との差分を算出する。便宜的に、この差分のことを「第２の演算位置誤差」と呼称する。加減算部８により算出された第２の演算位置誤差“Ｐ２err”は、カルマンフィルター係数調整部９Ａに出力される。
　カルマンフィルター係数調整部９Ａは、ＩＮＳ演算誤差推定部９１及びフィルター係数算出部９３を有し、第１のカルマンフィルター処理に係るフィルター係数を調整する。本実施形態では、フィルター係数算出部９３を、第１のカルマンフィルター処理部５Ａの機能部として図示・説明する。
　フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１により推定されたＩＮＳ演算誤差を利用して、第１のカルマンフィルター処理に係るフィルター係数を算出する。調整するフィルター係数は、例えば、状態“Ｘ1”,“Ｘ2”、及び、誤差共分散行列“Ｐ11”，“Ｐ22”，“Ｐ21”，“Ｐ12”である。フィルター係数の調整方法については詳細に後述する。
　ＩＮＳ演算誤差推定部９１は、ＩＮＳ演算結果に含まれる誤差であるＩＮＳ演算誤差を推定する機能部であり、例えば、位置誤差加重平均部９１１と、重み設定部９１３と、誤差変換部９１５とを有する。
　位置誤差加重平均部９１１は、重み設定部９１３により設定された重みを用いて、第１の演算位置誤差“Ｐ１err”と第２の演算位置誤差“Ｐ２err”とを加重平均して、ＩＮＳユニット２Ａの演算位置誤差（慣性測位誤差）を算出する。具体的には、例えば次式（７）に従って平均位置誤差“ａｖｅＰerr”を算出する。
Figure JPOXMLDOC01-appb-M000007
 　但し、“α”は、第１の演算位置誤差“Ｐ１err”に対する重みであり、“０≦α≦１”である。第２の演算位置誤差“Ｐ２err”に対する重みは“１－α”である。
　重み設定部９１３は、測位環境やＧＰＳユニット３Ａの計測結果の信頼性といった複数の要素に基づいて、位置誤差加重平均部９１１の加重平均の重み“α”を設定する。重み設定方法については詳細に後述する。
　誤差変換部９１５は、第１のカルマンフィルター処理部５Ａから入力した誤差共分散“Ｐ”を用いて、位置誤差加重平均部９１１から入力した平均位置誤差“ａｖｅＰerr”をＩＮＳ演算誤差に変換する。ＩＮＳ演算誤差の変換方法についても詳細に後述する。
　図３は、第１のカルマンフィルター処理部５Ａの入出力データの説明図である。状態“Ｘ”と、入力“Ｕ”と、観測量“Ｚ”との対応関係のテーブルを図示している。カップリングには種々の方式が存在する。その中でも、ルーズカップリング（疎結合）と呼ばれる方式と、タイトカップリング（密結合）と呼ばれる方式とが一般的に用いられる。
　ルーズカップリング方式は、ＧＰＳとＩＮＳとの結び付きが比較的弱いカップリング方式である。この方式では、例えば、状態“Ｘ”をＩＮＳ演算誤差（ＩＮＳ演算位置誤差、ＩＮＳ演算速度誤差、ＩＮＳ演算姿勢角誤差、ＩＮＳ加速度バイアス、ＩＮＳ角速度バイアス、クロックバイアス等）とする。また、入力“Ｕ”をＩＮＳ演算結果（ＩＮＳ演算位置、ＩＮＳ演算速度、ＩＮＳ演算姿勢角等）とし、観測量“Ｚ”をＧＰＳ演算結果（ＧＰＳ演算速度等）や運動モデルに基づく制約条件（停止時又は移動時）とする。
　タイトカップリング方式は、ＧＰＳとＩＮＳとの結び付きが比較的強いカップリング方式である。この方式では、例えば、入力“Ｕ”及び状態“Ｘ”を上記と同様とし、観測量 “Ｚ”をＧＰＳメジャメント情報（擬似距離変化率等）や運動モデルに基づく制約条件とする。
　上記の状態“Ｘ”、入力“Ｕ”、観測量“Ｚ”の各成分は、適宜追加／削除可能である。例えば、状態“Ｘ”について、バイアス成分を削除して、ＩＮＳ演算位置誤差、ＩＮＳ演算速度誤差及びＩＮＳ演算姿勢角誤差を状態“Ｘ”の成分としてもよい。また、観測量 “Ｚ”について、ＧＰＳ演算速度又は擬似距離変化率と、運動モデルに基づく制約条件とを併用することとしてもよいし、何れか一方のみを用いることとしてもよい。
　１－２．重み設定方法
　次に、重み“α”の設定方法を説明する。本実施形態では、比較的高い値として定められた高設定値“αhigh”と、中程度の値として定められた中設定値“αmiddle”と、比較的低い値として定められた低設定値“αlow”との３種類の値の何れかを重み“α”に設定する。これらの具体的な値は、適宜定めることとしてよい。重み“α”は「０～１」の範囲で、「αhigh＝０．９，αmiddle＝０．５，αlow＝０．１」といった値や、「αhigh＝０．７５，αmiddle＝０．５，αlow＝０．２５」といった値を定めておくことができる。
　図４は、重み設定に係る第１の重み設定条件を定めた第１の重み設定条件テーブルの一例を示す図である。第１の重み設定条件テーブルには、低重み設定条件と、高重み設定条件とが対応付けて定められている。低重み設定条件は、重み“α”に低設定値“αlow”を設定するための条件である。高重み設定条件は、重み“α”に高設定値“αhigh”を設定するための条件である。低重み設定条件及び高重み設定条件の何れの条件も成立しない場合は、重み“α”に中設定値“αmiddle”を設定する。
　第１の重み設定条件は、測位環境に基づき定められた条件である。低重み設定条件には、測位環境がマルチパス環境であることが定められており、高重み設定条件には、測位環境がオープンスカイ環境であることが定められている。
　測位環境がマルチパス環境である場合は、間接波の影響により、観測される擬似距離に誤差が生ずる。そのため、ＧＰＳユニット３Ａにより演算される位置の正確性が低下する。第２のカルマンフィルター処理部７ＡではＧＰＳ演算位置を観測量“Ｚ”とするため、ＧＰＳ演算位置の正確性が低いと、第２の演算位置にもその影響が現れる。
　この場合、第２の演算位置とＩＮＳ演算位置との差分として算出される第２の演算位置誤差“Ｐ２err”は、マルチパスの影響を含む値となる。従って、測位環境がマルチパス環境である場合は、マルチパスの影響が顕著に反映された第２の演算位置誤差“Ｐ２err”を重視するために第２の演算位置誤差“Ｐ２err”に対する重み“１－α”を大きくする。これは、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくすることに相当する。
　図５は、重み設定に係る第２の重み設定条件を定めた第２の重み設定条件テーブルの一例を示す図である。第２の重み設定条件テーブルには、条件の番号である条件Ｎｏと、低重み設定条件と、高重み設定条件とが対応付けて定められている。
　条件“Ａ”は、ＧＰＳ演算位置の信頼性に関する条件である。低重み設定条件には「ＧＰＳ演算位置の信頼性＝低」が定められており、高重み設定条件には「ＧＰＳ演算位置の信頼性＝高」が定められている。
　ＧＰＳ演算位置の信頼性が低いと、第２のカルマンフィルター処理部７Ａで演算される第２の演算位置の正確性が低下する。そのため、第２の演算位置とＩＮＳ演算位置との差分として算出される第２の演算位置誤差“Ｐ２err”には、その影響が顕著に反映される。そこで、ＧＰＳ演算位置の信頼性が低い場合には、第２の演算位置誤差“Ｐ２err”を重視するために、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
　ＧＰＳ演算位置の信頼性は、ＧＰＳ衛星信号の受信環境や信号強度、ＧＰＳ衛星の天空配置、位置の急激な変化の検出といった複数の要素を総合的に考慮して判定することができる。例えば、ＧＰＳ衛星の天空配置が良好でない場合（例えばＤＯＰ（Dilution Of Precision）値が大きい場合）や、大きな位置飛び（真の位置から大きく離れた位置が演算結果として得られること）が発生した場合、高度方向の急激な位置変化が発生した場合に、ＧＰＳ演算位置の信頼性が低いと判定することができる。
　条件“Ｂ”は、ＧＰＳメジャメント情報の信頼性に関する条件である。低重み設定条件には「ＧＰＳメジャメント情報の信頼性＝低」が定められており、高重み設定条件には「ＧＰＳメジャメント情報の信頼性＝高」が定められている。ＧＰＳメジャメント情報の信頼性が低いと、演算されるＧＰＳ演算位置の信頼性が低下する。従って、この場合は条件 “Ａ”と同様に、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
　例えば、アーバンキャニオン環境等においてＧＰＳメジャメント情報に突発的に異常値が混入したような場合や、擬似距離変化率（レンジレート）と擬似距離（シュードレンジ）との相関関係に矛盾が生じている場合に、ＧＰＳメジャメント情報の信頼性は低いと判定する。
　条件“Ｃ”は、第２の演算位置の信頼性に関する条件である。低重み設定条件には「第２の演算位置の信頼性＝低」が定められており、高重み設定条件には「第２の演算位置の信頼性＝高」が定められている。第２のカルマンフィルター処理で演算された第２の演算位置の信頼性が低い場合は、第２の演算位置とＩＮＳ演算位置との差分として算出される第２の演算位置誤差“Ｐ２err”を重視するように、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
　例えば、移動体の速度はゼロであるか微小値であるにも関わらず第２の演算位置が不連続的に変化している場合や、第２の演算位置に突発的な位置飛びが発生した場合に、第２の演算位置の信頼性は低いと判定する。
　条件“Ｄ”は、時刻情報の信頼性に関する条件である。低重み設定条件には「時刻情報の信頼性＝高」が定められており、高重み設定条件には「時刻情報の信頼性＝低」が定められている。時刻情報の信頼性が低いと、ＧＰＳユニット３Ａにより演算されるＧＰＳ演算位置の信頼性が低下する。そのため、条件“Ａ”と同様に、第１の演算位置誤差“Ｐ１err”に対する重み“α”を小さくする。
　例えば、ＧＰＳの時刻同期誤差が所定の閾値を超えている（或いは閾値以上）場合に、時刻情報の信頼性は低いと判定する。
　上記の複数の条件を用いて重み“α”を設定する。まず、複数の条件それぞれについて、判定項目の信頼性を数値化する。例えば、判定項目の信頼性が最も低い状態を信頼度“０”、信頼性が最も高い状態を信頼度“１”として、各判定項目の信頼度を「０～１」の範囲で数値化する。
　そして、例えば次式（８）に従って、重み設定指標値“Ｗ”を算出する。
Figure JPOXMLDOC01-appb-M000008
 　但し、“ｋ＝ｋA，ｋB，ｋC，・・・”は、各々の条件“Ａ，Ｂ，Ｃ，・・・”の信頼度に対する重みであり、“ｋA＋ｋB＋ｋC＋・・・＝Σｋ＝１”である。単純化するのであれば“ｋA＝ｋB＝ｋC＝・・・＝１／Ｎ”としてもよい。但し、“Ｎ”は、重み設定に用いる条件の総数である。
　重み設定指標値“Ｗ”に基づいて、設定する重み“α”を決定する。例えば、重み設定指標値“Ｗ”が第１の閾値“θ1”よりも小さい（又は以下）場合は、重み“α”を低設定値とする（α＝αlow）。また、重み設定指標値“Ｗ”が第２の閾値“θ2（＞θ1）”よりも大きい（又は以上）場合は、重み“α”を高設定値とする（α＝αhigh）。また、上記以外の場合には、重み“α”を中設定値とする（α＝αmiddle）。
　図４の第１の重み設定条件と、図５の第２の重み設定条件とは、それぞれ単体で用いてもよいし、併用してもよい。つまり、測位環境に基づいて重みを設定して加重平均することも可能であるし、ＧＰＳユニット３Ａの計測結果の信頼性等に基づいて重みを設定して加重平均することも可能であるし、これらの条件を併用して重みを設定して加重平均することも可能である。条件を併用する場合は、第１の重み設定条件の判定結果と第２の重み設定条件の判定結果とをＡＮＤ条件又はＯＲ条件として、重み“α”を設定することとしてもよい。
　また、第１の重み設定条件を第２の重み設定条件に含めて判定を行うことも可能である。具体的には、例えば、測位環境が最も良好な状態（例えばオープンスカイ環境）を“１”、測位環境が最も悪い状態（例えばアーバンキャニオン環境）を“０”として、測位環境の良否を数値化する。そして、数値化した測位環境の良否を含めて、式（８）の重み設定指標値“Ｗ”を算出することとしてもよい。
　１－３．ＩＮＳ演算誤差推定方法
　次に、ＩＮＳ演算誤差の推定方法を説明する。平均位置誤差をＩＮＳ演算誤差に変換する方法としては、例えばカルマンフィルターを用いることができる。
　最初に、次式（９）に従って、観測量“Ｚ2”と予測した観測量“Ｈ22Ｘ2”との差分“Ｖ2”を算出する。
Figure JPOXMLDOC01-appb-M000009
　式（９）において、観測量“Ｚ2”は平均位置誤差“ａｖｅＰerr”である。すなわち、 “Ｚ2＝ａｖｅＰerr”である。また、“Ｈ22Ｘ2”は観測量“Ｚ2”の予測値であり、平均位置誤差“ａｖｅＰerr”の予測値に相当する。つまり、式（２）で与えられる状態“Ｘ2”に観測行列“Ｈ22”を作用させて平均位置誤差“ａｖｅＰerr”を予測することを示す式である。観測行列“Ｈ22”は、状態“Ｘ2”から観測量“Ｚ2”への変換行列である。
　次に、次式（１０）及び（１１）に従って、カルマンゲイン“Ｋ12”及び“Ｋ22”を算出する。
Figure JPOXMLDOC01-appb-M000010
Figure JPOXMLDOC01-appb-M000011
　“Ｋ12”は差分“Ｖ2”から状態“Ｘ1”を演算するためのカルマンゲインであり、“Ｋ22”は差分“Ｖ2”から状態“Ｘ2”を演算するためのカルマンゲインである。また、“Ｒ2”は観測量“Ｚ2”の観測誤差（観測ノイズ）である。
　そして、上記のカルマンゲイン“Ｋ12”及び“Ｋ22”を用いて、状態“Ｘ1”及び“Ｘ2”を、次式（１２）及び（１３）に従って算出する。
Figure JPOXMLDOC01-appb-M000012
Figure JPOXMLDOC01-appb-M000013
　式（１２）に従って算出された状態“Ｘ1”の成分のうち、速度誤差及び姿勢角誤差を、推定速度誤差“Ｖerr”及び推定姿勢角誤差“Ａerr”として第１のカルマンフィルター処理部５Ａにフィードバックする。また、式（１３）に従って算出された状態“Ｘ2”の成分である位置誤差を、推定位置誤差“Ｐerr”として第１のカルマンフィルター処理部５Ａにフィードバックする。また、カルマンゲイン“Ｋ12”及び“Ｋ22”を第１のカルマンフィルター処理部５Ａにフィードバックする。
　１－４．フィルター係数調整方法
　次に、フィルター係数の調整方法を説明する。フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１から入力したＩＮＳ演算誤差（推定位置誤差“Ｐerr”、推定速度誤差 “Ｖerr”及び推定姿勢角誤差“Ａerr”）やカルマンゲイン“Ｋ12”及び“Ｋ22”を用いて、フィルター係数を算出する。
　第１に、フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１から入力した推定速度誤差“Ｖerr”及び推定姿勢角誤差“Ａerr”を、状態“Ｘ1”のうちの対応する成分（速度誤差“（δＶE，δＶN，δＶU）”、姿勢角誤差“（Ψx，Ψy，Ψz）”）に加味して、状態“Ｘ1”を算出・更新する。また、ＩＮＳ演算誤差推定部９１から入力した推定位置誤差“Ｐerr”を状態“Ｘ2”の各成分（位置誤差“（δＰE，δＰN，δＰU）”）に加味して、状態“Ｘ2”を算出・更新する。
　第２に、フィルター係数算出部９３は、カルマンゲイン“Ｋ12”及び“Ｋ22”を用いて、例えば次式（１４）～（１７）に従って誤差共分散行列“Ｐ11”，“Ｐ22”，“Ｐ21”，“Ｐ12”をそれぞれ算出・更新する。
Figure JPOXMLDOC01-appb-M000014
Figure JPOXMLDOC01-appb-M000015
Figure JPOXMLDOC01-appb-M000016
Figure JPOXMLDOC01-appb-M000017
　２．実験結果
　図６及び図７は、上記の位置算出方法を用いて位置算出を行った実験結果の一例を示す図である。第１の位置算出装置１Ａを自動車に設置し、都市部の道路を自動車で走行する実験を行った。走行した都市部は高層ビルが数多く存在するマルチパス環境である。図６及び図７の都市部の道路を反時計回りに周回して位置を算出し、その軌跡を描いてみた。スタート，ゴール地点は、図面中央下部である。
　図６（１）は、自動車の真の軌跡（参照軌跡）である。図６（２）は、ＧＰＳユニット３Ａの出力であるＧＰＳ演算位置の軌跡である。図６（３）は、第２のカルマンフィルター処理部７Ａの出力である第２の演算位置の軌跡である。
　図７（１）は、演算係数の調整を行わない場合の第１のカルマンフィルター処理部５Ａの出力である第１の演算位置の軌跡（演算係数調整なし）である。図７（２）は、演算係数の調整は行ったが、重み“α”の設定を行わなかった場合の第１の演算位置の軌跡（演算係数調整あり、重み設定なし）である。図７（３）は、演算係数の調整及び重み“α”の設定を両方とも行った場合の第１の演算位置の軌跡（演算係数調整あり、重み設定あり）である。
　最初に、図６（２）のＧＰＳ演算位置の軌跡を見ると、所々に大きく蛇行しており、位置誤差が非常に大きくなっていることがわかる。これは、マルチパスの影響によりＧＰＳの演算精度が低下したことによるものである。図６（３）の第２の演算位置の軌跡を見ると、ＧＰＳ演算位置の軌跡と比べると滑らかな軌跡を描いているが、それでもやはり、図６（１）の真の軌跡に沿った軌跡とはなっていない。
　次に、図７（１）の第１の演算位置の軌跡（演算係数調整なし）を見ると、点線部分Ｓ１において、真の軌跡に対してバイアスがかかったような軌跡が得られている。これに本実施形態の演算係数の調整を追加すると、図７（２）の結果が得られる。この図７（２）の第１の演算位置の軌跡（演算係数調整あり、重み設定なし）を見ると、演算係数の調整により位置算出精度が改善されていることがわかる。しかし、それでもなお、点線部分Ｓ２において真の軌跡からの位置ズレが生じている。
　これにさらに本実施形態の重み“α”の設定を追加すると、図７（３）の結果が得られる。この図７（３）の第１の演算位置の軌跡（演算係数調整あり、重み設定あり）を見ると、図６（１）の真の軌跡に沿った正確な軌跡が得られていることがわかる。特に、図７（１）の点線部分Ｓ１や図７（２）の点線部分Ｓ２に対応する部分についても位置算出精度が改善されている。この実験結果から、本実施形態の位置算出方法が有効であることが実証された。
　３．実施例
　次に、上記の位置算出装置を備えた電子機器の実施例について説明する。ここでは、位置算出装置を具備するカーナビゲーション装置の実施例を説明する。但し、本発明を適用可能な実施例が以下説明する実施例に限定されるわけではないことは勿論である。
　３－１．システム構成
　図８は、本実施例におけるナビゲーションシステム１０００のシステム構成の説明図である。ナビゲーションシステム１０００は、移動体の一種である四輪自動車（以下、単に「自動車」と称す。）に、位置算出装置１を具備したカーナビゲーション装置１００が設置されてなる。
　カーナビゲーション装置１００は、自動車に設置され、自動車の運転者に対するナビゲーションを行う電子機器である。カーナビゲーション装置１００は、ＩＮＳユニット２Ａと、ＧＰＳユニット３Ａとを備える。
　カーナビゲーション装置１００は、ＩＮＳユニット２Ａから入力したＩＮＳ演算結果と、ＧＰＳユニット３Ａから入力したＧＰＳ演算結果とを用いて、原理で説明した位置算出方法に従って自動車の位置を算出する。そして、算出位置をプロットしたナビゲーション画面を生成して、表示部３０であるディスプレイに表示させる。
　３－２．機能構成
　図９は、カーナビゲーション装置１００の機能構成の一例を示すブロック図である。カーナビゲーション装置１００は、ＩＮＳユニット２Ａと、ＧＰＳユニット３Ａと、処理部１０と、操作部２０と、表示部３０と、通信部４０と、記憶部５０とを備える。
　処理部１０は、記憶部５０に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置１００の各部を統括的に制御する制御装置であり、ＣＰＵ（Central Processing Unit）等のプロセッサーを有して構成される。処理部１０は、記憶部５０に記憶されたナビゲーションプログラム５１に従ってナビゲーション処理を行い、自動車の現在位置を指し示した地図を表示部３０に表示させる。
　操作部２０は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部１０に出力する。この操作部２０の操作により、目的地の入力等の各種指示入力がなされる。
　表示部３０は、ＬＣＤ（Liquid Crystal Display）等により構成され、処理部１０から入力される表示信号に基づいた各種表示を行う表示装置である。表示部３０には、ナビゲーション画面等が表示される。
　通信部４０は、処理部１０の制御に従って、装置内部で利用される情報をインターネット等の通信ネットワークを介して外部とやりとりするための通信装置である。この通信には、公知の無線通信技術を適用可能である。
　記憶部５０は、ＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の記憶装置によって構成され、カーナビゲーション装置１００のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
　記憶部５０には、プログラムとして、処理部１０により読み出され、ナビゲーション処理（図１０参照）として実行されるナビゲーションプログラム５１が記憶されている。ナビゲーションプログラム５１は、ＩＮＳ演算誤差推定処理（図１１参照）として実行されるＩＮＳ演算誤差推定プログラム５１１をサブルーチンとして含む。
　また、記憶部５０には、データとして、例えば、重み設定条件テーブル５３と、ＩＮＳ演算結果５５と、ＧＰＳ演算結果５６と、第１の演算結果５７と、第２の演算結果５８と、演算係数５９とが記憶される。
　重み設定条件テーブル５３は、第１の重み設定条件テーブル（図４参照）や第２の重み設定条件テーブル（図５参照）といった重み設定用の条件が定められたテーブルである。
　３－３．処理の流れ
　図１０は、処理部１０が、記憶部５０に記憶されたナビゲーションプログラム５１に従って実行するナビゲーション処理の流れを示すフローチャートである。
　先ず、処理部１０は、ＩＮＳユニット２Ａ及びＧＰＳユニット３Ａから、ＩＮＳ演算結果５５及びＧＰＳ演算結果５６の取得を開始する（ステップＡ１）。そして、処理部１０は、第１のカルマンフィルター処理を実行する（ステップＡ３）。
　第１のカルマンフィルター処理では、前回のＩＮＳ演算誤差推定処理（ステップＡ９）の推定結果を用いてフィルター係数を調整し、記憶部５０の演算係数５９を更新する。具体的には、原理で説明したように、推定したＩＮＳ演算誤差を用いて状態“Ｘ”を調整するとともに、例えば式（１４）～（１７）に従って誤差共分散“Ｐ”を調整する。そして、調整したフィルター係数を用いてカルマンフィルター処理を行い、その演算結果で記憶部５０の第１の演算結果５７を更新する。
　次いで、処理部１０は、第１のカルマンフィルター処理で演算した演算位置に対するマップマッチング処理等を行い、その結果で表示部３０のナビゲーション画面を表示更新する（ステップＡ５）。
　その後、処理部１０は、第２のカルマンフィルター処理を実行する（ステップＡ７）。処理部１０は、第２のカルマンフィルター処理の演算結果で記憶部５０の第２の演算結果５８を更新する。そして、処理部１０は、記憶部５０に記憶されたＩＮＳ演算誤差推定プログラム５１１に従ってＩＮＳ演算誤差推定処理を行う（ステップＡ９）。
　図１１は、ＩＮＳ演算誤差推定処理の流れを示すフローチャートである。
　先ず、処理部１０は、重み設定処理を行う（ステップＢ１）。具体的には、記憶部５０に記憶されている重み設定条件テーブル５３を参照して、原理で説明した手法に従って重み“α”を設定する。
　次いで、処理部１０は、位置誤差加重平均処理を行う（ステップＢ３）。具体的には、第１のカルマンフィルター処理で演算した第１の演算位置誤差“Ｐ１err”と、第２のカルマンフィルター処理で演算した第２の演算位置誤差“Ｐ２err”とを、ステップＢ１で設定した重み“α”を用いて式（７）に従って加重平均して、平均位置誤差“ａｖｅＰerr”を算出する。
　その後、処理部１０は、誤差変換処理を行う（ステップＢ５）。具体的には、ステップＢ３で算出した平均位置誤差“ａｖｅＰerr”を、式（９）～式（１３）に従ってＩＮＳ演算誤差に変換する。そして、処理部１０は、ＩＮＳ演算誤差推定処理を終了する。
　図１０のナビゲーション処理に戻り、ＩＮＳ演算誤差推定処理を行った後、処理部１０は、処理を終了するか否かを判定する（ステップＡ１１）。例えば、操作部２０を介してユーザーによりナビゲーションの終了指示操作がなされた場合に、ナビゲーション処理を終了すると判定する。
　処理を終了しないと判定した場合は（ステップＡ１１；Ｎｏ）、処理部１０は、ステップＡ３に戻る。また、処理を終了すると判定した場合は（ステップＡ１１；Ｙｅｓ）、ナビゲーション処理を終了する。
　４．作用効果
　本実施形態によれば、位置算出装置１において、移動体に設置された慣性測位用ユニット２の計測結果を用いて、少なくとも移動体の位置を演算する第１の演算処理が第１の演算処理部５により実行される。また、第１の演算処理の結果と、移動体に設置された衛星測位用ユニット３の計測結果とを用いて、移動体の位置を演算する第２の演算処理が第２の演算処理部７により実行される。そして、第１の演算処理の結果と、第２の演算処理の結果とを用いて、第１の演算処理に係る演算係数が演算係数調整部９により調整される。
　例えば、位置算出装置１を適用した第１の位置算出装置１Ａでは、第１のカルマンフィルター処理部５Ａにおいて、ＩＮＳユニットの演算結果（ＩＮＳ演算結果）を入力とし、ＧＰＳユニット３Ａの演算速度（ＧＰＳ演算速度）を観測量とするカルマンフィルター処理が実行される。また、第２のカルマンフィルター処理部７Ａにおいて、第１のカルマンフィルター処理部５Ａの演算結果（第１の演算結果）を入力とし、ＧＰＳユニット３Ａの演算位置（ＧＰＳ演算位置）を観測量とするカルマンフィルター処理が実行される。
　加減算部８では、ＩＮＳ演算位置と第２の演算位置との差分が、第２の演算位置誤差として算出される。ＩＮＳ演算誤差推定部９１では、重み設定部９１３により、第１の演算位置誤差と第２の演算位置誤差とを加重平均する際の重みが設定される。そして、第１の演算位置誤差と第２の演算位置誤差とが、位置誤差加重平均部９１１により加重平均される。そして、加重平均結果である平均位置誤差を用いて、誤差変換部９１５により平均位置誤差がＩＮＳ演算誤差に変換され、その変換結果が第１のカルマンフィルター処理部５Ａにフィードバックされる。
　重み設定部９１３は、例えば、測位環境に基づき定められた第１の重み設定条件や、ＧＰＳ演算位置の信頼性、ＧＰＳメジャメント情報の信頼性、第２の演算位置の信頼性、時刻情報の信頼性といった複数の要素に基づき定められた第２の重み設定条件に従って、加重平均の重みを設定する。これにより、加重平均の重みを適正化し、ＩＮＳ演算位置に含まれる誤差を適切に推定することができる。
　また、誤差変換部９１５は、第１のカルマンフィルター処理部５Ａから入力した誤差共分散と、位置誤差加重平均部９１１から入力した平均位置誤差とを用いて、カルマンフィルターを利用した変換演算を行って、ＩＮＳ演算誤差を推定する。カルマンフィルターを利用することで、平均位置誤差からＩＮＳ演算誤差を適切に推定することができる。
　また、フィルター係数算出部９３は、ＩＮＳ演算誤差推定部９１により推定されたＩＮＳ演算誤差やカルマンゲインを用いて、第１のカルマンフィルター処理で用いるフィルター係数を算出・調整する。例えば、第１のカルマンフィルター処理で用いる状態（状態ベクトル）や誤差共分散（誤差共分散行列）を算出・調整する。このようにして調整したフィルター係数を用いて第１のカルマンフィルター処理を行うことで、移動体の位置を高い正確性で求めることが可能となる。
　５．変形例
　本発明を適用可能な実施例は、上記の実施例に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明するが、上記の実施例と同一の構成要素については同一の符号を付して説明を省略し、上記の実施例とは異なる部分を中心に説明する。
　５－１．ユニット
　上記の実施形態では、衛星測位用ユニット３として、ＧＰＳを適用したＧＰＳユニット３Ａを例に挙げて説明したが、ＷＡＡＳ（Wide Area Augmentation System）、ＱＺＳＳ（Quasi Zenith Satellite System）、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の他の衛星測位システムを適用したユニットとしてもよいことは勿論である。
　また、上記の実施形態では、慣性測位用ユニット２として、ＩＮＳユニット２Ａを適用した場合を例に挙げて説明したが、ＩＮＳメジャメント情報（加速度や角速度）を計測する慣性センサーや慣性計測ユニット（ＩＭＵ）を慣性測位用ユニット２としてもよい。この場合は、慣性測位用ユニット２により計測されたＩＮＳメジャメント情報を入力Ｕとして、第１の演算処理部５が第１の演算処理を行うように構成すればよい。
　５－２．演算処理
　第１の演算処理部５及び第２の演算処理部７が行う演算処理は、カルマンフィルター処理に限定されるわけではない。例えば、第２の演算処理について、シグマ点（シグマポイント）を利用したシグマポイントフィルター処理や、回帰フィルター処理を適用することとしてもよい。
　５－３．座標系
　上記の実施形態では、ＩＮＳユニット２Ａは、移動体の位置、速度及び姿勢角をＥＮＵ座標系で演算するものとして説明したが、ＮＥＤ座標系や、ＥＣＥＦ座標系で演算することとしてもよいことは勿論である。また、第１の演算処理や第２の演算処理についても、ＥＮＵ座標系で演算するのではなく、ＮＥＤ座標系やＥＣＥＦ座標系で演算してもよい。
　５－４．重み設定条件
　上記の実施形態で例示した重み設定条件はあくまでも一例であり、適宜追加／削除可能である。例えば、ＩＮＳユニット２Ａの演算結果の信頼性に基づく条件を定めておいてもよい。ＩＮＳユニット２Ａが有する慣性センサー（ジャイロセンサーや加速度センサー）の計測結果に大きな誤差が混入すると、ＩＮＳユニット２Ａの演算精度が低下する。この場合、ＩＮＳ演算結果を入力とする第１の演算処理の演算精度が低下する。
　その結果、第１の演算処理で演算される第１の演算位置誤差“Ｐ１err”には、その影響が顕著に現れる。そこで、ＩＮＳユニット２Ａの演算結果の信頼性が低い場合は、第１の演算位置誤差“Ｐ１err”を重視して加重平均するように、第１の演算位置誤差“Ｐ１err”に対する重み“α”を大きくすると効果的である。この場合は、高重み設定条件として「ＩＮＳユニットの信頼性＝低」を定めておけばよい。
　また、上記の実施形態では、第２の重み設定条件に関して、複数の条件“Ａ，Ｂ，Ｃ，Ｄ，・・・”を組み合わせて重み“α”を設定する方法を説明した。しかし、複数の条件 “Ａ，Ｂ，Ｃ，Ｄ，・・・”をそれぞれ単体で用いて重み“α”を設定するといったことも当然に可能である。
　５－５．処理主体
　上記の実施形態では、ＩＮＳユニット２ＡがＩＮＳ演算を行い、ＧＰＳユニット３ＡがＧＰＳ演算を行うものとして説明した。しかし、電子機器の処理部１０がＩＮＳ演算やＧＰＳ演算を実行する構成としてもよい。
　この場合は、ＩＮＳユニット２Ａは、ＩＮＳメジャメント情報（加速度や角速度等）を出力する。また、ＧＰＳユニット３Ａは、ＧＰＳメジャメント情報（コード位相やドップラー周波数、擬似距離、擬似距離変化率等）を出力する。
　そして、電子機器の処理部１０は、ＩＮＳユニット２Ａから入力したＩＮＳメジャメント情報を用いてＩＮＳ演算を行うとともに、ＧＰＳユニット３Ａから入力したＧＰＳメジャメント情報を用いてＧＰＳ演算を行う。そして、処理部１０は、ＩＮＳ演算結果及びＧＰＳ演算結果を用いて、第１及び第２の演算処理を行う。
　５－６．電子機器
　上記の実施例では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
　また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、ＰＤＡ（Personal Digital Assistant）といった他の電子機器についても本発明を同様に適用して、当該電子機器の位置算出を実現することが可能である。
　１、１Ａ　位置算出装置、　２　慣性測位用ユニット、　２Ａ　ＩＮＳユニット、　３　衛星測位用ユニット、　３Ａ　ＧＰＳユニット、　５　第１の演算処理部、　５Ａ　第１のカルマンフィルター処理部、　７　第２の演算処理部、　７Ａ　第２のカルマンフィルター処理部、　９　演算係数調整部、　９Ａ　カルマンフィルター係数調整部、　１０　処理部、　２０　操作部、　３０　表示部、　４０　通信部、　５０　記憶部、　１００　カーナビゲーション装置、　１０００　ナビゲーションシステム。

Claims (8)

  1. 　移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行することと、
    　前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行することと、
    　前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整することと、
    　を含む位置算出方法。
  2. 　前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、
    　前記第１の演算処理は、演算した第１の演算位置に内在する第１の位置誤差を推定する所定の誤差推定演算を含み、
    　前記調整することは、
    　前記第１の位置誤差と、前記第２の演算処理で求めた第２の演算位置とを用いて、前記慣性測位位置に内在する慣性測位誤差を推定することと、
    　前記慣性測位誤差を用いて前記演算係数を調整することと、
    　を含む、
    　請求項１に記載の位置算出方法。
  3. 　前記推定することは、
    　前記慣性測位位置と前記第２の演算位置との差分を算出することと、
    　前記第１の位置誤差と前記差分とを平均処理して前記慣性測位誤差を算出することと、
    　を含む、
    　請求項２に記載の位置算出方法。
  4. 　前記平均処理は、測位環境と前記衛星測位用ユニットの計測結果の信頼性とのどちらか又は両方に基づいて、前記第１の位置誤差と前記差分との重みを設定して加重平均する処理である、
    　請求項３に記載の位置算出方法。
  5. 　前記慣性測位用ユニットの計測結果には慣性測位位置が含まれ、
    　前記第１の演算処理は、前記慣性測位位置を入力とするカルマンフィルター処理である、
    　請求項１～４の何れか一項に記載の位置算出方法。
  6. 　前記衛星測位用ユニットの計測結果には速度が含まれ、
    　前記カルマンフィルター処理は、前記速度を観測量とする処理である、
    　請求項５に記載の位置算出方法。
  7. 　前記衛星測位用ユニットの計測結果には衛星測位位置が含まれ、
    　前記第２の演算処理は、前記第１の演算処理の結果を入力とし、前記衛星測位位置を観測量とするカルマンフィルター処理である、
    　請求項１～６の何れか一項に記載の位置算出方法。
  8. 　移動体に設置された慣性測位用ユニットの計測結果を用いて、少なくとも前記移動体の位置を演算する第１の演算処理を実行する第１の演算処理部と、
    　前記第１の演算処理の結果と、前記移動体に設置された衛星測位用ユニットの計測結果とを用いて、前記移動体の位置を演算する第２の演算処理を実行する第２の演算処理部と、
    　前記第１の演算処理の結果と、前記第２の演算処理の結果とを用いて、前記第１の演算処理に係る演算係数を調整する調整部と、
    　を備えた位置算出装置。
PCT/JP2012/002967 2011-05-10 2012-05-02 位置算出方法及び位置算出装置 WO2012153501A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
EP12782570.1A EP2717014B1 (en) 2011-05-10 2012-05-02 Position calculation method and position calculation device
US14/116,750 US9026362B2 (en) 2011-05-10 2012-05-02 Position calculating method and position calculating device

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2011105614A JP5742450B2 (ja) 2011-05-10 2011-05-10 位置算出方法及び位置算出装置
JP2011-105614 2011-05-10

Publications (1)

Publication Number Publication Date
WO2012153501A1 true WO2012153501A1 (ja) 2012-11-15

Family

ID=47138989

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2012/002967 WO2012153501A1 (ja) 2011-05-10 2012-05-02 位置算出方法及び位置算出装置

Country Status (4)

Country Link
US (1) US9026362B2 (ja)
EP (1) EP2717014B1 (ja)
JP (1) JP5742450B2 (ja)
WO (1) WO2012153501A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2016085060A (ja) * 2014-10-23 2016-05-19 株式会社小野測器 速度計測装置

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6260983B2 (ja) * 2013-05-24 2018-01-17 株式会社Ihi 自己位置推定装置及び方法
US9309631B2 (en) 2014-03-26 2016-04-12 Caterpillar Trimble Control Technologies Llc Enhanced control of road construction equipment
US9574320B2 (en) * 2014-03-26 2017-02-21 Trimble Navigation Limited Blended position solutions
JP6139722B1 (ja) * 2016-02-19 2017-05-31 ヤフー株式会社 推定装置、推定方法及び推定プログラム
JP6712037B2 (ja) * 2017-01-27 2020-06-17 株式会社ＦＡＤｒｏｎｅ 慣性計測方法と慣性計測装置及び慣性計測プログラム
WO2019049599A1 (ja) * 2017-09-05 2019-03-14 Ｎｅｃソリューションイノベータ株式会社 情報処理装置、情報処理システム、測位結果出力方法およびプログラムを格納した非一時的なコンピュータ可読媒体
US11513234B2 (en) 2019-09-13 2022-11-29 Apple Inc. Estimating device position in multipath environments
US11593411B2 (en) * 2019-09-13 2023-02-28 International Business Machines Corporation Historical augmentation of electronic maps
WO2021059346A1 (ja) * 2019-09-24 2021-04-01 日本電信電話株式会社 測位システム、機器、サーバ装置、測位方法及びプログラム
US20210278847A1 (en) * 2020-03-05 2021-09-09 Analog Devices, Inc. Trusted motion unit
CN113359167A (zh) * 2021-04-16 2021-09-07 电子科技大学 一种通过惯性测量参数将gps与激光雷达融合定位的方法
US11550064B1 (en) * 2021-06-30 2023-01-10 Guangzhou Xiaopeng Autopilot Technology Co., Ltd. Apparatus, system and method for providing global localization output and application of same

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010014715A (ja) * 2008-07-02 2010-01-21 O2 Micro Inc 全地球測位システム及び推測航法（ｇｐｓ＆ｄｒ）一体型ナビゲーションシステム
US20100019963A1 (en) 2006-06-15 2010-01-28 Uti Limited Partnership Vehicular navigation and positioning system
JP2010151459A (ja) * 2008-12-24 2010-07-08 Seiko Epson Corp 位置算出方法及び位置算出装置
JP2011013115A (ja) * 2009-07-02 2011-01-20 Seiko Epson Corp 位置算出方法及び位置算出装置
JP2011013228A (ja) * 2010-08-30 2011-01-20 Seiko Epson Corp 位置算出方法及び位置算出装置

Family Cites Families (56)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5646857A (en) * 1995-03-31 1997-07-08 Trimble Navigation Limited Use of an altitude sensor to augment availability of GPS location fixes
US5862511A (en) * 1995-12-28 1999-01-19 Magellan Dis, Inc. Vehicle navigation system and method
US6393046B1 (en) * 1996-04-25 2002-05-21 Sirf Technology, Inc. Spread spectrum receiver with multi-bit correlator
US6234799B1 (en) 1998-04-06 2001-05-22 American Gnc Corporation Real-time IMU simulator
US6449559B2 (en) * 1998-11-20 2002-09-10 American Gnc Corporation Fully-coupled positioning process and system thereof
US6608589B1 (en) * 1999-04-21 2003-08-19 The Johns Hopkins University Autonomous satellite navigation system
ATE345487T1 (de) * 1999-09-16 2006-12-15 Sirf Tech Inc Navigationssystem und -verfahren zum verfolgen der position eines objektes
US6731237B2 (en) * 1999-11-09 2004-05-04 The Charles Stark Draper Laboratory, Inc. Deeply-integrated adaptive GPS-based navigator with extended-range code tracking
US6489922B1 (en) * 2000-04-22 2002-12-03 American Gnc Corporation Passive/ranging/tracking processing method for collision avoidance guidance and control
US6735523B1 (en) * 2000-06-19 2004-05-11 American Gnc Corp. Process and system of coupled real-time GPS/IMU simulation with differential GPS
US6408245B1 (en) 2000-08-03 2002-06-18 American Gnc Corporation Filtering mechanization method of integrating global positioning system receiver with inertial measurement unit
WO2002052225A2 (en) * 2000-12-22 2002-07-04 The Charles Stark Draper Laboratory, Inc. Geographical navigation using multipath wireless navigation signals
US6424914B1 (en) * 2000-12-26 2002-07-23 American Gnc Corporation Fully-coupled vehicle positioning method and system thereof
US6493631B1 (en) * 2001-05-31 2002-12-10 Mlho, Inc. Geophysical inertial navigation system
US6853909B2 (en) * 2001-12-03 2005-02-08 Applanix Corporation, Inc Walking stick navigator for position determination
US6697736B2 (en) * 2002-02-06 2004-02-24 American Gnc Corporation Positioning and navigation method and system thereof
KR100498480B1 (ko) * 2003-01-23 2005-07-01 삼성전자주식회사 Ｇｐｓ 위성 신호를 이용한 위치추정방법 및 위치추정장치
JP4199553B2 (ja) * 2003-02-03 2008-12-17 古野電気株式会社 ハイブリッド航法装置
KR100520166B1 (ko) * 2003-03-14 2005-10-10 삼성전자주식회사 네비게이션시스템에서 이동체의 위치검출장치 및 그 방법
WO2005072458A2 (en) * 2004-01-28 2005-08-11 Delorme Publishing Company, Inc. Method and device for processing raw gps data
FR2866423B1 (fr) * 2004-02-13 2006-05-05 Thales Sa Dispositif de surveillance de l'integrite des informations delivrees par un systeme hybride ins/gnss
US8013789B2 (en) * 2004-10-06 2011-09-06 Ohio University Systems and methods for acquisition and tracking of low CNR GPS signals
US7395156B2 (en) * 2005-06-23 2008-07-01 Raytheon Company System and method for geo-registration with global positioning and inertial navigation
US7474962B2 (en) * 2005-07-13 2009-01-06 Honeywell International Inc. Methods and systems of relative navigation for shipboard landings
KR101216551B1 (ko) * 2006-03-06 2012-12-31 콸콤 인코포레이티드 측정 스티칭을 사용한 위치 결정 방법
JP4830559B2 (ja) * 2006-03-16 2011-12-07 セイコーエプソン株式会社 測位装置及び測位方法
US7865299B2 (en) * 2006-09-14 2011-01-04 Toyota Motor Engineering & Manufacturing North America, Inc. Method and system for predicting a future position of a vehicle using numerical integration
US7626544B2 (en) * 2006-10-17 2009-12-01 Ut-Battelle, Llc Robust low-frequency spread-spectrum navigation system
US8068984B2 (en) * 2006-10-17 2011-11-29 Ut-Battelle, Llc Triply redundant integrated navigation and asset visibility system
US8493267B2 (en) * 2006-11-10 2013-07-23 Qualcomm Incorporated Method and apparatus for position determination with extended SPS orbit information
US7439907B2 (en) * 2006-11-20 2008-10-21 Sirf Technology Holdihgs, Inc. Navigation signal receiver trajectory determination
US7835863B2 (en) * 2007-04-18 2010-11-16 Mitac International Corporation Method and system for navigation using GPS velocity vector
US7869948B2 (en) * 2007-04-27 2011-01-11 Sirf Technology, Inc. Method and apparatus in positioning without broadcast ephemeris
US8260036B2 (en) * 2007-05-09 2012-09-04 Honeywell International Inc. Object detection using cooperative sensors and video triangulation
US20090287414A1 (en) * 2007-05-14 2009-11-19 Zupt, Llc System and process for the precise positioning of subsea units
US7855678B2 (en) * 2007-05-16 2010-12-21 Trimble Navigation Limited Post-mission high accuracy position and orientation system
US7586441B2 (en) * 2007-06-27 2009-09-08 Mediatek Inc. Methods and apparatuses for searching for satellite signals
JP4964047B2 (ja) * 2007-07-12 2012-06-27 アルパイン株式会社 位置検出装置及び位置検出方法
JP4976948B2 (ja) * 2007-07-25 2012-07-18 古野電気株式会社 姿勢計測装置
JP4453728B2 (ja) * 2007-08-07 2010-04-21 株式会社デンソー 位置補正装置
US20090093959A1 (en) * 2007-10-04 2009-04-09 Trimble Navigation Limited Real-time high accuracy position and orientation system
US7994971B2 (en) * 2008-01-09 2011-08-09 Mayflower Communications Company, Inc. GPS-based measurement of roll rate and roll angle of spinning platforms
US9366763B2 (en) * 2009-02-04 2016-06-14 Qualcomm Incorporated Method and apparatus for position determination with hybrid SPS orbit data
US8134497B2 (en) * 2008-09-30 2012-03-13 Trimble Navigation Limited Method and system for location-dependent time-specific correction data
JP2010117148A (ja) * 2008-11-11 2010-05-27 Seiko Epson Corp 位置算出方法及び位置算出装置
JP2010117147A (ja) * 2008-11-11 2010-05-27 Seiko Epson Corp 位置算出方法及び位置算出装置
US7978130B1 (en) * 2009-05-01 2011-07-12 Coherent Navigation, Inc. Practical method for upgrading existing GNSS user equipment with tightly integrated Nav-Com capability
US8296065B2 (en) * 2009-06-08 2012-10-23 Ansaldo Sts Usa, Inc. System and method for vitally determining position and position uncertainty of a railroad vehicle employing diverse sensors including a global positioning system sensor
EP2339378B1 (en) * 2009-12-17 2013-03-20 u-blox AG Hybrid satellite positioning receiver
JP5521531B2 (ja) * 2009-12-18 2014-06-18 セイコーエプソン株式会社 位置算出方法及び位置算出システム
US8704707B2 (en) * 2010-06-02 2014-04-22 Qualcomm Incorporated Position determination using measurements from past and present epochs
JP2012215491A (ja) * 2011-04-01 2012-11-08 Seiko Epson Corp 位置算出方法及び位置算出装置
JP5842363B2 (ja) * 2011-04-01 2016-01-13 セイコーエプソン株式会社 位置算出方法及び位置算出装置
EP2530488B1 (en) * 2011-06-01 2016-04-13 u-blox AG Hybrid satellite positioning with prediction
US8626198B2 (en) * 2011-11-16 2014-01-07 Qualcomm Incorporated Characterizing an indoor structure based on detected movements and/or position locations of a mobile device
US8983492B2 (en) * 2012-06-21 2015-03-17 Qualcomm Incorporated Methods and apparatuses for affecting a motion model within a mobile device

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100019963A1 (en) 2006-06-15 2010-01-28 Uti Limited Partnership Vehicular navigation and positioning system
JP2010014715A (ja) * 2008-07-02 2010-01-21 O2 Micro Inc 全地球測位システム及び推測航法（ｇｐｓ＆ｄｒ）一体型ナビゲーションシステム
JP2010151459A (ja) * 2008-12-24 2010-07-08 Seiko Epson Corp 位置算出方法及び位置算出装置
JP2011013115A (ja) * 2009-07-02 2011-01-20 Seiko Epson Corp 位置算出方法及び位置算出装置
JP2011013228A (ja) * 2010-08-30 2011-01-20 Seiko Epson Corp 位置算出方法及び位置算出装置

Non-Patent Citations (1)

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

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2016085060A (ja) * 2014-10-23 2016-05-19 株式会社小野測器 速度計測装置

Also Published As

Publication number Publication date
US20140214317A1 (en) 2014-07-31
JP2012237606A (ja) 2012-12-06
EP2717014A4 (en) 2014-10-29
EP2717014A1 (en) 2014-04-09
US9026362B2 (en) 2015-05-05
JP5742450B2 (ja) 2015-07-01
EP2717014B1 (en) 2017-08-09

Similar Documents

Publication Publication Date Title
JP5742450B2 (ja) 位置算出方法及び位置算出装置
JP5606656B2 (ja) 測位装置
US9714841B2 (en) Satellite navigation/dead-reckoning navigation integrated positioning device
US8560234B2 (en) System and method of navigation based on state estimation using a stepped filter
JP6094026B2 (ja) 姿勢判定方法、位置算出方法及び姿勢判定装置
US8600660B2 (en) Multipath modeling for deep integration
US9423509B2 (en) Moving platform INS range corrector (MPIRC)
WO2012137415A1 (ja) 位置算出方法及び位置算出装置
JP5842363B2 (ja) 位置算出方法及び位置算出装置
JP5602070B2 (ja) 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム
WO2009006341A1 (en) Compensation for mounting misalignment of a navigation device
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
WO2016203744A1 (ja) 測位装置
JP5119967B2 (ja) 測位方法、プログラム及び測位装置
JP2012154769A (ja) 加速度検出方法、位置算出方法及び加速度検出装置
WO2012172780A1 (ja) 位置算出方法及び位置算出装置
JP2015102330A (ja) 移動情報算出装置、移動情報算出方法、移動情報算出プログラム、および移動体
JP2013108930A (ja) 慣性航法演算方法及び慣性航法演算装置
Komori et al. Initial Study on Spoofing Detection Using IMU and GNSS Compass
JP2013195325A (ja) 移動体の測位方法、および移動体の測位装置
JP2012198068A (ja) 方位測定装置

Legal Events

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

Ref document number: 12782570

Country of ref document: EP

Kind code of ref document: A1

REEP Request for entry into the european phase

Ref document number: 2012782570

Country of ref document: EP

WWE Wipo information: entry into national phase

Ref document number: 2012782570

Country of ref document: EP

NENP Non-entry into the national phase

Ref country code: DE

WWE Wipo information: entry into national phase

Ref document number: 14116750

Country of ref document: US