JP6689659B2 - 位置推定装置、及び、推定方法 - Google Patents

Description

本発明は、位置推定装置、及び、推定方法に関する。

本技術分野の背景技術として、特開２０１２−２１５４９１号公報（特許文献１）がある。特許文献１には、「ＧＰＳユニット３とＩＮＳ（登録商標）ユニット５とを備えた位置算出装置１において、位置算出精度判定部６は、位置算出精度を判定する。影響度合設定部７は、位置算出精度判定部６により判定された位置算出精度に基づいて、ＧＰＳ計測結果のＩＮＳ計測結果に対する影響度合を設定する。カップリング処理部９は、影響度合設定部７により設定された影響度合に基づいてＧＰＳ計測結果とＩＮＳ計測結果とのカップリング処理を実行して、移動体の位置を算出する。」と記載されている。

特開２０１２−２１５４９１号公報

一般に、カルマンフィルタに基づいて位置や方位等の現在地に関する状態を推定する際、センサーやＧＰＳ信号等に基づき観測される位置や方位等の観測量の誤差は、誤差の平均がゼロとなる白色性の誤差であることが前提とされる。しかしながら、ＧＰＳ信号に基づき観測される位置の誤差は、誤差の平均がゼロでない有色性の誤差である場合がある。特許文献１では、ＧＰＳ信号に基づき観測される位置の誤差が有色性の誤差であることを考慮しておらず、カルマンフィルタにより現在地に関する状態を精度よく推定できない。
そこで、本発明は、カルマンフィルタにより現在地に関する状態を精度よく推定できるようにすることを目的とする。

上記目的を達成するために、位置推定装置は、ＧＰＳ信号を受信するＧＰＳ受信部と、前記ＧＰＳ受信部が受信した前記ＧＰＳ信号に基づくＧＰＳ測位位置を含む観測量を観測する観測部と、前記観測部が観測する前記観測量とカルマンフィルタとに基づいて、現在地に関する状態量を推定する推定部と、を備え、前記推定部は、前記状態量の予測値と、前記予測値の誤差とを算出し、前記予測値と、前記予測値の誤差と、前記観測部が観測する前記観測量の誤差とに基づいて、前記状態量の推定値と、前記推定値の誤差とを算出し、前記推定値と、前記推定値の誤差とを算出する際、前記ＧＰＳ信号の受信に係る第１の影響に基づく誤差が前記ＧＰＳ測位位置に対して発生してから、前記第１の影響と異なる第２の影響に基づく誤差が前記ＧＰＳ測位位置に対して発生するまでの期間に基づく重み付けを、前記ＧＰＳ測位位置の誤差に付与する、ことを特徴とする。

本発明によれば、カルマンフィルタにより現在地に関する状態を精度よく推定できる。

ナビゲーション装置の構成を示すブロック図である。 ナビゲーション装置の動作を示すフローチャートである。 車両状態推定処理における推定部の動作を示すフローチャートである。 相関時定数を説明するための図である。 推定した車両の状態に基づくマップマッチングの処理を説明するための図である。

図１は、ナビゲーション装置１（位置推定装置、車載装置）の構成を示すブロック図である。

ナビゲーション装置１は、車両に搭載される車載装置であり、車両に搭乗しているユーザの操作に従って、地図の表示や、地図における車両の現在位置の表示、目的地までの経路探索、経路案内等を実行する。なお、ナビゲーション装置１は、車両のダッシュボード等に固定されてもよいし、車両に対し着脱可能なものであっても良い。

図１に示すように、ナビゲーション装置１は、制御部２と、記憶部３と、タッチパネル４と、ＧＰＳ受信部５と、車速センサー６と、ジャイロセンサー７と、加速度センサー８とを備える。

制御部２は、ＣＰＵや、ＲＯＭ、ＲＡＭ、その他の制御回路等を備え、ナビゲーション装置１の各部を制御する。制御部２は、ＲＯＭや記憶部３等に記憶された制御プログラムを実行することにより、後述する観測部２１、及び、推定部２２として機能する。

記憶部３は、ハードディスクや、ＥＥＰＲＯＭ等の不揮発性メモリを備え、データを書き換え可能に記憶する。記憶部３は、制御部２が実行する制御プログラムのほか、地図データ３ａを記憶する。地図データ３ａは、交差点やその他の道路網上の接続点を示すノードに関するノード情報や、ノードとノードとの道路区間を示すリンクに関するリンク情報、地図の表示に係る情報等を有する。リンク情報は、リンク毎に、少なくとも、リンクの位置に関する情報と、リンクの方位に関する情報とを含む。

タッチパネル４は、表示パネル４ａと、タッチセンサー４ｂとを備える。表示パネル４ａは、液晶ディスプレイやＥＬ（Electro Luminescent）ディスプレイ等により構成され、制御部２の制御の下、各種情報を表示パネル４ａに表示する。タッチセンサー４ｂは、表示パネル４ａに重ねて配置され、ユーザのタッチ操作を検出し、タッチ操作された位置を示す信号を制御部２に出力する。制御部２は、タッチセンサー４ｂからの入力に基づいて、タッチ操作に対応する処理を実行する。

ＧＰＳ受信部５は、ＧＰＳアンテナ５ａを介してＧＰＳ衛星から送信されるＧＰＳ信号を受信する。そして、ＧＰＳ受信部５は、受信したＧＰＳ信号に基づいて、車両とＧＰＳ衛星間の距離及び距離の変化率を所定数以上の衛星に対して測定することにより、少なくとも車両の位置（ＧＰＳ測位位置）と車両の進行方向の方角（以下、「車両の方位」と表現する）を算出する。また、ＧＰＳ受信部５は、車両の位置、及び、車両の方位を算出する際、受信したＧＰＳ信号と、前回算出した、車両の位置、及び、車両の方位を加味して算出する。以下の説明において、ＧＰＳ信号に基づく車両の位置を、ＧＰＳ車両位置と表現する。ＧＰＳ受信部５は、ＧＰＳ車両位置と、車両の方位とを算出する際、ＧＰＳ車両位置の誤差と、車両の方位の誤差とを算出する。ＧＰＳ受信部５は、ＧＰＳ車両位置を示す情報、ＧＰＳ車両位置の誤差を示す情報、車両の方位を示す情報、及び、車両の方位の誤差を示す情報を、制御部２に出力する。

車速センサー６は、車両の車速を検出して、検出した車速を示す信号を制御部２に出力する。

ジャイロセンサー７は、例えば振動ジャイロにより構成され、車両の回転による角速度を検出する。ジャイロセンサー７は、検出した角速度を示す信号を制御部２に出力する。

加速度センサー８は、車両に作用する加速度（例えば、進行方向に対する車両の傾き）を検出する。加速度センサー８は、検出した加速度を示す信号を制御部２に出力する。

図１に示すように、制御部２は、観測部２１と、推定部２２とを備える。

観測部２１は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８から出力される信号と、ＧＰＳ受信部５から出力される情報とに基づき、車両の変動に関する観測量を観測する。

観測部２１は、車速センサー６から出力される車速を示す信号に基づき、観測量として車両の速度を観測する。観測部２１は、所定の演算により、車速を示す信号から、車両の速度と車両の速度の誤差とを算出する。

また、観測部２１は、ジャイロセンサー７から出力される角速度を示す信号に基づき、車両の回転による角速度を観測量として観測する。観測部２１は、所定の演算により、角速度を示す信号から、車両の角速度と車両の角速度の誤差とを算出する。

また、観測部２１は、加速度センサー８から出力される加速度を示す信号に基づき、観測量として車両の加速度を観測する。観測部２１は、所定の演算により、加速度を示す信号から、車両の加速度と車両の加速度の誤差とを算出する。

また、観測部２１は、ＧＰＳ受信部５から出力されたＧＰＳ車両位置を示す情報と車両の方位を示す情報とに基づき、ＧＰＳ車両位置と車両の方位とを観測量として観測する。

推定部２２は、観測部２１が観測する車両の速度、車両の加速度、車両の角速度、車両の位置、及び、車両の方位に基づき、カルマンフィルタにより、車両の現在地に関する状態を示す状態量を推定する。本実施形態では、推定部２２は、車両の速度、車両の角速度、車両の位置、及び、車両の方位を、車両の状態量として推定する。

後述するが、制御部２は、推定部２２が推定した車両の状態量に基づき、マップマッチングの対象となるリンクの評価を実行する。

なお、推定部２２が推定する車両の速度は、車両の状態量に相当する。また、観測部２１が観測する車両の速度は、観測量に相当する。同様に、推定部２２が推定する車両の角速度は、車両の状態量に相当する。また、観測部２１が観測する車両の角速度は、観測量に相当する。

ここで、カルマンフィルタによる基本的な車両の状態量の推定について説明する。

本実施形態では、カルマンフィルタにより推定する車両の状態量は、車両の位置、車両の方位、車両の速度、及び、車両の角速度である。以下に、カルマンフィルタにより推定する車両の各状態量を示す。

ｘ：車両の位置のｘ座標
ｙ：車両の位置のｙ座標
θ：車両の方位
ｖ：車両の速度
ω：車両の角速度

ここで、車両の状態量をベクトル表記した状態ベクトルを、（ｘ、ｙ、θ、ｖ、ω）とすると、車両の状態量についての状態方程式は、式（１）で表される。

添え字のｋ＋１及びｋは、時刻を示す。例えば、式（１）の左辺である（ｘ_ｋ＋１、ｙ_ｋ＋１、θ_ｋ＋１、ｖ_ｋ＋１、ω_ｋ＋１）は、時刻ｋ＋１における車両の状態量を示す。式（１）において、右辺の第２項であるｑ_ｋは、システム雑音（平均０、誤差共分散行列であるＱ_ｋを有する正規分布Ｎ（０、Ｑ_ｋ））である。誤差共分散行列とは、分散と共分散との行列である。

前述した通り、観測部２１は、観測量として、車両の速度、車両の角速度、車両の加速度、ＧＰＳ車両位置、及び、車両の方位を観測する。前述した通り、観測部２１は、車両の速度について、車速センサー６からの出力に基づき観測する。また、観測部２１は、車両の角速度について、ジャイロセンサー７からの出力に基づき観測する。また、観測部２１は、車両の加速度について、加速度センサー８からの出力に基づき観測する。また、観測部２１は、ＧＰＳ受信部５からの出力に基づき、ＧＰＳ車両位置、及び、車両の方位について、観測する。以下に、観測部２１が観測する観測量を示す。なお、以下では、観測量として車両の速度、車両の角速度、ＧＰＳ車両位置、及び、車両の方位を例示する。

ｖ^ＰＬＳ：車速センサー６からの出力に基づき観測される車両の速度
ω^ＧＹＲ：ジャイロセンサー７からの出力に基づき観測される車両の角速度
ｘ^ＧＰＳ：ＧＰＳ受信部５からの出力に基づき観測されるＧＰＳ車両位置のｘ座標
ｙ^ＧＰＳ：ＧＰＳ受信部５からの出力に基づき観測されるＧＰＳ車両位置のｙ座標
θ^ＧＰＳ：ＧＰＳ受信部５からの出力に基づき観測される車両の方位

ここで、上記の観測量をベクトル表記した観測ベクトルを、（ｖ^ＰＬＳ、ω^ＧＹＲ、ｘ^ＧＰＳ、ｙ^ＧＰＳ、θ^ＧＰＳ）とすると、観測量についての観測方程式は、式（２）で表される。

式（２）において、ｒ_ｋは、観測雑音（平均０、誤差共分散行列であるＲ_ｋを有する正規分布Ｎ（０、Ｒ_ｋ））である。

以下、カルマンフィルタについて、車両の状態量を予測する予測処理と、車両の状態量を推定する推定処理とに分けて説明する。

なお、以下の説明において、添え字のｋ＋１｜ｋが付与された値は、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における予測値を示す。また、添え字のｋ＋１｜ｋ＋１が付与された値は、時刻ｋ＋１までの情報に基づき推定された時刻ｋ＋１における推定値を示す。また、添え字のｋ｜ｋが付与された値は、時刻ｋまでの情報に基づき推定された時刻ｋにおける推定値を示す。

＜予測処理＞
カルマンフィルタにおいて予測処理は、車両の状態量の予測値（以下、「車両状態予測値」と表現する）と、車両状態予測値の誤差共分散行列（予測値の誤差）と、を算出する処理である。車両状態予測値は、式（３）に基づき算出される。なお、誤差共分散行列の算出とは、誤差共分散行列の各成分の値の算出を示す。

式（３）では、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における車両状態予測値の算出を示す。この車両状態予測値は、式（３）の右辺に示すように、時刻ｋまでの情報に基づき推定された時刻ｋにおける車両の状態量の推定値により算出される。例えば、車両の位置のｘ座標の予測値を示すｘ_{ｋ＋１｜ｋ}は、時刻ｋまでの情報に基づき推定された時刻ｋにおける、車両の位置のｘ座標（ｘ_ｋ｜ｋ）の推定値と、車両の方位（θ_ｋ｜ｋ）の推定値と、車両の速度（ｖ_ｋ｜ｋ）の推定値とに基づき算出される。なお、式（３）において、Ｔは、車速センサー６、ジャイロセンサー７、及び、加速度センサー８からの出力に基づく観測量を、観測部２１が観測する間隔を示す。

なお、時刻ｋ−１までの情報に基づき予測された時刻ｋの車両状態予測値を算出する場合、式（３）において時刻を１ステップずつ下げた式により算出できる。つまり、時刻ｋ−１までの情報に基づき予測された時刻ｋにおける車両状態予測値は、時刻ｋ−１までの情報に基づき推定された時刻ｋ−１における車両の状態量の推定値に基づき算出される。

車両状態予測値の誤差共分散行列は、式（４）に基づき算出される。誤差共分散行列は、本実施形態において、車両の状態量に関する分散及び共分散の行列である。分散は、誤差を二乗したものである。つまり、車両の状態量の分散は、車両の状態量の誤差を二乗したものである。したがって、車両状態予測値の誤差共分散行列を算出することは、車両状態予測値の誤差を算出することに相当する。

式（４）において、Ｐは、誤差共分散行列を示す。式（４）の左辺は、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における誤差共分散行列を示す。式（４）の左辺に示す誤差共分散行列は、時刻ｋまでの情報に基づき推定された時刻ｋにおける誤差共分散行列に基づき算出される。なお、Ｆは、式（１）の状態方程式から求められるヤコビ行列を示す。また、Ｆにおける添え字のＴは、転置行列を示す。また、Ｑは、システム雑音の誤差共分散行列を示す。

なお、時刻ｋ−１までの情報に基づき予測された時刻ｋにおける車両状態予測値の誤差共分散行列を算出する場合、式（４）においてＰの添え字の時刻を１ステップずつ下げた式により算出できる。つまり、時刻ｋ−１までの情報に基づき予測された時刻ｋにおける車両状態予測値の誤差共分散行列は、時刻ｋ−１までの情報に基づき推定された時刻ｋ−１における車両の状態量の推定値の誤差共分散行列により算出される。

このように、予測処理では、例えば、時刻ｋ＋１における車両の状態量を予測する場合、時刻ｋまでの情報に基づき算出された時刻ｋ＋１における車両状態予測値と、時刻ｋまでの情報に基づき算出された時刻ｋ＋１における車両状態予測値の誤差共分散行列とを算出する。つまり、予測処理は、車両の状態量の確立分布を予測している。

＜推定処理＞
次に、推定処理について説明する。
カルマンフィルタにおいて推定処理は、予測処理にて算出した車両状態予測値、及び、車両状態予測値の誤差共分散行列に基づいて、車両の状態量の推定値（以下、「車両状態推定値」と表現する）と、車両状態推定値の誤差共分散行列（推定値の誤差）とを算出する処理である。

推定処理では、式（５）により観測残差が算出される。観測残差とは、観測量と、車両状態予測値から算出される観測量に対応する値との差である。

式（５）において、左辺は、観測残差をベクトル表記した観測残差ベクトルである。また、式（５）において、右辺の第１項は、時刻ｋにおいて観測部２１が観測した観測量の観測ベクトルである。また、式（５）において、右辺の第２項は、予測処理で予測した車両状態予測値に、観測方程式から求められる観測行列を示す「Ｈ」を掛けたものである。

車両状態推定値は、式（５）に示す観測残差を用いて、式（６）により算出される。

式（６）では、時刻ｋ＋１までの情報に基づき予測された時刻ｋ＋１における車両状態推定値を示す。この車両状態推定値は、式（６）の右辺に示すように、時刻ｋまでの情報に基づき推定された時刻ｋ＋１における車両状態予測値を、観測残差より補正して算出される。なお、時刻ｋまでの情報に基づき予測された時刻ｋにおける車両状態推定値を算出する場合、時刻ｋ−１までの情報に基づき予測された時刻ｋにおける車両状態予測値を観測残差により補正することで算出される。

前述の通り、式（６）は、観測残差を用いて、車両状態予測値を補正し、車両状態推定値を算出する式を示す。式（６）に示すように、車両状態予測値を観測残差にて補正する際の補正係数としてＫ_ｋが用いられる。Ｋ_ｋは、カルマンゲインと呼ばれ、式（７）で表される。

式（７）において、Ｒ_ｋは、観測雑音の誤差共分散行列である。本実施形態では、観測雑音の誤差共分散行列とは、観測量の誤差を示す誤差共分散行列である。以下、当該誤差共分散行列を、観測量の誤差共分散行列と表現する。前述した通り、誤差を二乗した分散と、共分散との行列である。したがって、Ｒ_ｋは、観測量の誤差を二乗した分散と、共分散との行列である。つまり、観測量の誤差共分散行列は、観測量の誤差に相当する。なお、式（７）において、添え字の「−１」は、逆行列を示す。

式（７）に示すカルマンゲインＫ_ｋは、時刻ｋまでの情報に基づく時刻ｋ＋１における車両状態予測値の誤差共分散行列（Ｐ_{ｋ＋１｜ｋ}）と、時刻ｋにおいて観測された観測量の誤差共分散行列（Ｒ_ｋ）とに基づき算出される。

このカルマンゲインＫ_ｋは、式（６）において、車両状態推定値を算出する際、車両状態予測値を重視して算出するか、観測部２１が観測する観測量を重視して算出するかを決定する係数である。

例えば、車速センサー６、ジャイロセンサー７、及び、ＧＰＳ受信部５からの出力に基づく観測量の誤差が、車両状態予測値の誤差より充分に小さい場合、車両状態推定値は、観測量の誤差が充分に小さいことから、観測量となることが望ましい。つまり、観測量の誤差共分散行列であるＲ_ｋの値が、車両状態予測値の誤差共分散行列であるＰ_{ｋ＋１｜ｋ}の値より充分に小さい場合、車両状態予測値である式（６）の左辺は、観測量である式（５）の右辺第１項となることが望ましい。これは、車両状態予測値が、誤差が充分に小さい値、換言すると、精度の高い値となるためである。ここで、Ｒ_ｋの値が充分に小さい場合のカルマンゲインＫ_ｋをＫ_ｋ＝Ｈ^−１として式（６）に与えると、式（６）の右辺の第１項は、式（５）との関係に基づき、無くなる。つまり、車両状態推定値は、誤差が充分に小さい観測量となる。

また、例えば、車両状態予測値の誤差が、車速センサー６、ジャイロセンサー７、及び、ＧＰＳ受信部５からの出力に基づく観測量の誤差より充分に小さい場合、車両状態推定値は、車両状態予測値となることが望ましい。つまり、車両状態予測値の誤差共分散行列であるＰ_{ｋ＋１｜ｋ}の値が、観測量の誤差共分散行列であるＲ_ｋの値より充分に小さい場合、車両状態予測値である式（６）の左辺は、車両状態予測値である式（６）の右辺第１項となることが望ましい。これは、車両状態推定値が、観測量の誤差より充分に誤差が小さい値、換言すると、精度の高い値になるためである。ここで、車両状態予測値の誤差共分散行列がＲ_ｋの値より充分に小さい場合のカルマンゲインＫ_ｋとしてＫ_ｋ＝０として式（６）に与えると、車両状態推定値は、車両状態予測値となる。

このように、カルマンゲインＫ_ｋは、観測量の誤差共分散行列であるＲ_ｋ、及び、車両状態予測値の誤差共分散行列に応じて、車両状態推定値が適切な値となるように設定する係数である。すなわち、カルマンゲインＫ_ｋは、観測部２１が観測する観測量の誤差共分散行列と、車両状態予測値の誤差共分散行列とに基づいて、車両状態推定値が、観測量と車両状態予測値とのうち、誤差共分散行列が小さい、すなわち、誤差が小さいほうを重視した値となるよう設定する係数である。また、カルマンゲインＫ_ｋは、車両状態予測値の誤差共分散行列が正確に予測されていれば、換言すると、当該誤差共分散行列が正確に算出されている場合、車速センサー６、ジャイロセンサー７、及び、ＧＰＳ受信部５からの出力に基づく観測量の誤差共分散行列に応じて、車両状態推定値が適切な値となるように設定する。

車両状態推定値の誤差共分散行列は、式（８）により算出される。前述した通り、誤差共分散行列は、本実施形態では、車両の状態量に関する、分散及び共分散の行列である。車両の状態量の分散は、車両の状態量の誤差を二乗したものである。したがって、車両状態推定値の誤差共分散行列を算出することは、車両状態推定値の誤差を算出することに相当する。

式（８）において、Ｐは、式（４）と同様、誤差共分散行列を示す。また、式（８）において、Ｉは、単位行列を示す。式（８）の左辺は、時刻ｋ＋１までの情報に基づき推定された時刻ｋ＋１における誤差共分散行列を示す。式（８）の左辺に示す誤差共分散行列は、時刻ｋまでの情報に基づき予測された時刻ｋ＋１における誤差共分散行列に基づき算出される。

なお、時刻ｋまでの情報に基づき予測された時刻ｋにおける車両状態推定値の誤差共分散行列を算出する場合、式（８）においてＰの添え字の時刻を１ステップずつ下げた式により算出できる。つまり、時刻ｋまでの情報に基づき予測された時刻ｋにおける車両状態推定値の誤差共分散行列は、時刻ｋ−１までの情報に基づき推定された時刻ｋにおける車両状態予測値の誤差共分散行列により算出される。

式（８）は、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列に、（Ｉ−Ｋ_ｋＨ）を掛けた式である。式（８）に示すように、車両状態推定値の誤差共分散行列は、カルマンゲインＫ_ｋの値に依存する。

例えば、観測量の誤差、すなわち、Ｒ_ｋが充分に小さい場合のカルマンゲインＫ_ｋとしてＫ_ｋ＝Ｈ^−１を式（８）に与えると、車両状態推定値の誤差共分散行列は、零行列となる。これは、推定した車両状態推定値の誤差が充分に小さいことを示す。

また、例えば、観測量の誤差より車両状態予測値の誤差が充分に小さい場合のカルマンゲインの値としてＫ_ｋ＝０を与えることにより、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列になる。これは、車両状態推定値の誤差が、観測量の誤差より充分に小さい車両状態予測値の誤差であることを示す。

このように、式（８）において、カルマンゲインＫ_ｋは、観測量の誤差共分散行列と、車両状態予測値の誤差共分散行列に応じて、車両状態推定値の誤差共分散行列が適切になるように設定する。つまり、カルマンゲインＫ_ｋは、観測量の精度と車両状態予測値の精度とに基づいて、車両状態推定値の誤差共分散行列が適切になるように設定する。

以上のように、推定処理では、例えば、時刻ｋ＋１における車両の状態量を推定する場合、時刻ｋ＋１までの情報に基づき算出された時刻ｋ＋１における車両状態推定値と、時刻ｋ＋１までの情報に基づき算出された時刻ｋ＋１における車両状態推定値の誤差共分散行列とを算出する。つまり、推定処理は、予測処理で予測した車両の状態量の確立分布に基づいて、車両の状態量の確立分布を推定している。

以上の算出により、推定部２２は、カルマンフィルタによって、車両状態推定値と、車両状態推定値の誤差共分散行列とを算出することにより、車両の状態を推定する。

前述した通り、カルマンゲインＫ_ｋは、車両状態推定値と、車両状態推定値の誤差共分散行列とを適切に設定する係数である。つまり、車両の状態の推定の精度は、カルマンゲインＫ_ｋに依存する。カルマンゲインＫ_ｋは、前述した通り、観測量の誤差共分散行列と、車両状態予測値の誤差共分散行列とに応じて、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に設定する係数である。

ところで、カルマンフィルタでは、システム雑音、及び、観測雑音が白色性の雑音であることが前提とされる。つまり、カルマンフィルタにおいて誤差を雑音として扱う場合、当該誤差は、誤差の平均がゼロである白色性の誤差であることが前提となる。したがって、カルマンゲインＫ_ｋの算出において用いられる観測量の誤差は、白色性の誤差であることが前提とされる。しかしながら、観測部２１が観測する観測量のうち、少なくともＧＰＳ車両位置の誤差は、マルチパスや、ＧＰＳ衛星の衛星配置等の影響により、誤差の平均がゼロとならない有色性の誤差を示す場合がある。この場合に、カルマンゲインＫ_ｋは、観測量の誤差が有色性の誤差であるため、正確に算出されない。これは、車両状態推定値と車両状態推定値の誤差共分散行列とが精度よく算出できない、すなわち、車両の状態を精度よく推定できないことを示す。

そこで、本実施形態の推定部２２は、以下に示すように、ＧＰＳ車両位置の誤差を設定し、カルマンフィルタに基づいて車両の状態を推定する。

以下、車両の状態を推定する際のナビゲーション装置１の動作を通して、推定部２２による車両の状態の推定について説明する。

図２は、ナビゲーション装置１の動作を示すフローチャートである。

ナビゲーション装置１の観測部２１は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８から出力される信号に基づき、車両の速度、車両の角速度、及び、車両の加速度を観測する（ステップＳＡ１）。観測部２１は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８が信号を出力する度に、車両の速度、車両の角速度、及び、車両の加速度を観測する。つまり、観測部２１がこれらを観測する間隔は、車速センサー６、ジャイロセンサー７、及び、加速度センサー８が検出する間隔と同じ間隔である。

次いで、観測部２１は、ＧＰＳ受信部５の出力に基づき、ＧＰＳ車両位置、及び、車両の方位を観測する（ステップＳＡ２）。観測部２１は、ＧＰＳ車両位置を示す情報と車両の方位を示す情報とがＧＰＳ受信部５から出力される度に、ＧＰＳ車両位置、及び、車両の方位を観測する。つまり、観測部２１がＧＰＳ車両位置及び車両の方位を観測する間隔は、ＧＰＳ受信部５がＧＰＳ信号を受信する間隔となる。

次いで、ナビゲーション装置１の推定部２２は、観測部２１が観測した観測量に基づき、車両の状態を推定する車両状態推定処理を実行する（ステップＳＡ３）。

図３は、車両状態推定処理における推定部２２の動作を示すフローチャートである。

推定部２２は、予測処理を実行する（ステップＳＢ１）。前述した通り、予測処理は、車両状態予測値、及び、車両状態予測値の誤差共分散行列を算出する処理である。車両状態予測値は、式（３）によって算出される。車両状態予測値の誤差共分散行列は、式（４）によって算出される。

次いで、推定部２２は、推定処理を実行する（ステップＳＢ２）。推定部２２は、推定処理において、式（７）に基づいて、カルマンゲインＫ_ｋを算出し、算出したカルマンゲインＫ_ｋ、式（５）、式（６）、及び、式（８）に基づいて、車両状態推定値と車両状態推定値の誤差共分散行列とを算出する。

推定部２２は、式（７）に基づいて、カルマンゲインＫ_ｋを算出する際、観測量の誤差共分散行列が含む成分のうち、ＧＰＳ車両位置の誤差を、式（９）に示すように、設定する。なお、以下の説明において、便宜上、観測量の誤差共分散行列であるＲ_ｋに入力するＧＰＳ車両位置の誤差を、ＧＰＳ車両位置の観測誤差と表現し、観測部２１がＧＰＳ受信部５からの出力に基づき観測するＧＰＳ車両位置の誤差を、ＧＰＳ車両位置の測位誤差と表現する。

式（９）は、ＧＰＳ車両位置の観測誤差が、ＧＰＳ車両位置の測位誤差に√（２τ）を掛けたもので表されることを示す。これは、推定部２２が、ＧＰＳ車両位置の測位誤差に、τに基づく√（２τ）の重み付けを付与し、重み付けを付与したＧＰＳ車両位置の測位誤差をＧＰＳ車両位置の観測誤差として設定することを示す。

式（９）において、τは、相関時定数を示す。相関時定数とは、あるタイミング（第１のタイミング）から、当該あるタイミングにおいて受信したＧＰＳ信号が、観測部２１が観測するＧＰＳ車両位置の観測に反映しない当該あるタイミングと異なる他のタイミング（第２のタイミング）までの期間を示す。ここで、相関時定数について、詳述する。

図４は、相関時定数を説明するための図である。
図４では、車両が、方向ＹＺに延びる直線ＴＳ上を、方向ＹＺ１に向かって走行する場合を例示する。

タイミングＴ１（第１のタイミング）において、ＧＰＳ信号の受信に関する影響（以下、「ＧＰＳ受信影響」と表現する）（第１の影響）が発生したとする。ＧＰＳ受信影響は、例えば、マルチパスの影響や、ＧＰＳ衛星の衛星配置に起因したＧＰＳ信号の受信強度の低下の影響等が挙げられる。

図４では、タイミングＴ１における実際の車両の位置を、位置ＭＴ１とする。前述した通り、図４では、車両が直線ＴＳ上を走行する場合を例示するため、位置ＭＴ１は、直線ＴＳ上に位置する。

また、図４では、タイミングＴ１において観測部２１が観測するＧＰＳ車両位置を、位置ＮＴ１とする。図４に示すように、位置ＮＴ１は、直線ＴＳと同様に方向ＹＺに延びる仮想直線ＫＴ上に位置する。仮想直線ＫＴは、タイミングＴ１で発生したＧＰＳ受信影響によって実際の車両の位置とＧＰＳ車両位置とに誤差（以下、「離間誤差」と表現する）が発生した場合において、観測部２１が観測するＧＰＳ車両位置が位置する仮想の直線である。本実施形態では、仮想直線ＫＴは、直線ＴＳと並行な直線である。

このように、図４では、タイミングＴ１におけるＧＰＳ受信影響の発生により、観測部２１が、実際の車両の位置である位置ＭＴ１から距離ｌ１離れた位置ＮＴ１をＧＰＳ車両位置として観測する場合を例示する。つまり、図４では、タイミングＴ１において、ＧＰＳ受信影響により、観測部２１が観測するＧＰＳ車両位置と、実際の車両の位置との間に、距離ｌ１の離間誤差が発生したことを示している。

ＧＰＳ受信影響の発生によるＧＰＳ車両位置の測位誤差は、ＧＰＳ受信影響がない場合のＧＰＳ車両位置の測位誤差と比較して大きい誤差である。図４の場合、タイミングＴ１において観測部２１が観測するＧＰＳ車両位置の測位誤差、すなわち、位置ＮＴ１の誤差は、タイミングＴ１で発生したＧＰＳ受信影響により、当該ＧＰＳ受信影響がない場合のＧＰＳ車両位置の測位誤差と比較して大きい誤差である。

ここで、ＧＰＳ受信影響によりＧＰＳ車両位置の測位誤差が大きい誤差となることについて、以下に一例を挙げて説明する。
ＧＰＳ受信部５は、受信したＧＰＳ信号が含むＧＰＳ衛星の位置やＧＰＳ信号を送信した時刻等の情報や、ＧＰＳ信号を受信した時刻等に基づいて、ＧＰＳ衛星と車両との距離（以下、「擬似距離」と表現する）を測定する。ＧＰＳ受信部５は、複数のＧＰＳ衛星について得られた擬似距離に基づいて所定の演算することにより、ＧＰＳ車両位置を算出する。ここで、ＧＰＳ受信影響としてマルチパスの影響が発生したとする。マルチパスとは、ＧＰＳ衛星から直接到来する直接波と、建物等での反射による反射波との複数の経路からＧＰＳ信号を受信する現象である。このように複数の経路からＧＰＳ信号を受信するため、ＧＰＳ衛星から車両への到達時間は、直接波のＧＰＳ信号と反射波のＧＰＳ信号とにおいて異なる。そのため、反射波のＧＰＳ信号や、直接波と反射波が合成された合成波のＧＰＳ信号等に基づき算出される擬似距離は、直接波のＧＰＳ信号に基づき算出される擬似距離と比較して、擬似距離に大きな誤差が発生する。したがって、擬似距離に対する大きな誤差の発生によって、観測部２１が観測するＧＰＳ車両位置の測位誤差は、マルチパスの影響のないＧＰＳ車両位置の測位誤差と比較して大きくなる。

図４の説明に戻り、タイミングＴ２において、タイミングＴ１で発生したＧＰＳ受信影響が継続しているとする。図４では、タイミングＴ２における実際の車両の位置を、位置ＭＴ２とする。位置ＭＴ１と同様、位置ＭＴ２は、直線ＴＳ上に位置する。また、図４では、タイミングＴ２において観測部２１が観測するＧＰＳ車両位置を、位置ＮＴ２とする。

図４では、タイミングＴ２において、タイミングＴ１で発生したＧＰＳ受信影響により、観測部２１が、実際の車両の位置である位置ＭＴ２から距離ｌ２離れた位置ＮＴ２を、ＧＰＳ車両位置として観測する場合を例示する。つまり、図４では、タイミングＴ２において、タイミングＴ１で発生したＧＰＳ受信影響により、観測部２１が観測するＧＰＳ車両位置と、実際の車両の位置との間に、距離ｌ２の離間誤差が発生したことを示している。

前述した通り、ＧＰＳ受信部５は、前回算出したＧＰＳ車両位置を加味して、受信したＧＰＳ信号に基づくＧＰＳ車両位置を算出する。したがって、図４において、ＧＰＳ受信部５は、タイミングＴ２におけるＧＰＳ車両位置である位置ＮＴ２を算出する際、前回算出した位置、すなわち、タイミングＴ１において算出したＧＰＳ車両位置である位置ＮＴ１を加味して算出する。図４では、位置ＮＴ２が仮想直線ＫＴ上に位置することを示している。このように、前回算出した位置を加味することにより、ＧＰＳ受信部５は、実際の車両の位置の変化に対して、ＧＰＳ車両位置の変化が乖離することを抑制する。つまり、図４の場合、車両が直線ＴＳ上を方向ＹＺ１に向かって走行している場合、前回算出した位置を加味することにより、ＧＰＳ車両位置が、仮想直線ＫＴ上を方向ＹＺ１に向かって移動するように、ＧＰＳ車両位置を算出する。これによって、ＧＰＳ受信部５は、実際の車両の位置が直線ＴＳ上を方向ＹＺ１に向かって変化することに合わせて、ＧＰＳ車両位置が仮想直線ＫＴ上を方向ＹＺ１に向かって変化するよう、ＧＰＳ車両位置を算出する。図４では、位置ＮＴ１と位置ＮＴ２とが仮想直線ＫＴ上に位置する場合を例示するため、離間誤差である距離ｌ１と距離ｌ２とは、同じ値である。

このように、前回算出した位置を加味するため、ＧＰＳ受信部５は、タイミングＴ１で受信したＧＰＳ信号に基づき算出されたＧＰＳ車両位置、すなわち、位置ＮＴ１が反映されたＧＰＳ車両位置を算出する。つまり、観測部２１は、タイミングＴ２において、タイミングＴ１で受信したＧＰＳ信号が反映されたＧＰＳ車両位置、すなわち、位置ＮＴ２を観測する。

図４のタイミングＴ２において観測部２１が観測するＧＰＳ車両位置の測位誤差、すなわち、位置ＮＴ２の誤差は、タイミングＴ１で発生したＧＰＳ受信影響により、当該ＧＰＳ受信影響がない場合のＧＰＳ車両位置の測位誤差と比較して大きい誤差である。

ここで、位置ＮＴ１の誤差と、位置ＮＴ２の誤差とは、相関関係にある。これは、位置ＮＴ１の誤差及び位置ＮＴ２の誤差が、ＧＰＳ受信影響がない場合の誤差と比較して大きい誤差であることから、タイミングＴ１で発生したＧＰＳ受信影響に基づく誤差を含むためである。具体的には、位置ＮＴ１の誤差及び位置ＮＴ２の誤差は、同じＧＰＳ受信影響に基づく誤差を含むことから、互いに当該ＧＰＳ受信影響に応じた誤差となる。そのため、位置ＮＴ１の誤差及び位置ＮＴ２の誤差は、相関関係にある。このように、位置ＮＴ１の誤差及び位置ＮＴ２の誤差は、相関関係にあることから、相関性を有する誤差である。相関性を有する誤差は、誤差の平均がゼロにならない誤差である。したがって、位置ＮＴ１の誤差及び位置ＮＴ２の誤差は、誤差の平均がゼロにならない誤差、すなわち、有色性の誤差である。

タイミングＴ３においても、タイミングＴ１において発生したＧＰＳ受信影響が継続しているとする。図４では、タイミングＴ３における実際の車両の位置を、位置ＭＴ３とする。位置ＭＴ１、及び、位置ＭＴ２と同様、位置ＭＴ３は、直線ＴＳ上に位置する。また、図４では、タイミングＴ３において観測部２１が観測するＧＰＳ車両位置を、位置ＮＴ３とする。

図４では、タイミングＴ３において、タイミングＴ１で発生したＧＰＳ受信影響により、観測部２１が、実際の車両の位置である位置ＭＴ３から距離ｌ３離れた位置ＮＴ２を、ＧＰＳ車両位置として観測する場合を例示する。つまり、図４では、タイミングＴ３において、タイミングＴ１で発生したＧＰＳ受信影響により、観測部２１が観測するＧＰＳ車両位置と、実際の車両の位置との間に、距離ｌ３の離間誤差が発生したことを示している。

ＧＰＳ受信部５は、タイミングＴ３におけるＧＰＳ車両位置である位置ＮＴ３を算出する際、タイミングＴ２において算出したＧＰＳ車両位置を加味して算出する。タイミングＴ２において算出したＧＰＳ車両位置は、位置ＮＴ１を加味して算出したものである。したがって、ＧＰＳ受信部５は、タイミングＴ３において、タイミングＴ１で受信したＧＰＳ信号に基づき算出されたＧＰＳ車両位置が反映されたＧＰＳ車両位置を算出する。つまり、観測部２１は、タイミングＴ３において、タイミングＴ１で受信したＧＰＳ信号が反映されたＧＰＳ車両位置を観測する。

また、ＧＰＳ受信部５は、前回算出したＧＰＳ車両位置を加味するため、実際の車両の位置の変化に対して、ＧＰＳ車両位置の変化が乖離することを抑制した位置ＮＴ３を算出する。つまり、ＧＰＳ受信部５は、タイミングＴ１と同じＧＰＳ受信影響がタイミングＴ３においてあるため、仮想直線ＫＴ上に位置するように位置ＮＴ３を算出する。図４では、位置ＮＴ３が仮想直線ＫＴ上に位置する場合を例示するため、離間誤差である距離ｌ３は、距離ｌ１と距離ｌ２と同じ値である。

図４のタイミングＴ３において観測部２１が観測するＧＰＳ車両位置の測位誤差、すなわち、位置ＮＴ３の誤差は、タイミングＴ１で発生したＧＰＳ受信影響により、ＧＰＳ受信影響がない場合の誤差と比較して大きい誤差である。

ここで、位置ＮＴ２の誤差と、位置ＮＴ３の誤差とは、相関関係にある。これは、位置ＮＴ２の誤差及び位置ＮＴ３の誤差が、ＧＰＳ受信影響がない場合の誤差と比較して大きい誤差であることから、タイミングＴ１で発生したＧＰＳ受信影響に基づく誤差を含むためである。具体的には、位置ＮＴ２の誤差及び位置ＮＴ３の誤差は、タイミングＴ１で発生したＧＰＳ受信影響に基づく誤差を含むことから、互いに当該ＧＰＳ受信影響に応じた誤差となり、相関関係にある。このように、位置ＮＴ１の誤差及び位置ＮＴ２の誤差は、相関関係にあることから、相関性を有する誤差である。前述した通り、相関性を有する誤差は、誤差の平均がゼロにならない誤差である。したがって、位置ＮＴ１の誤差及び位置ＮＴ２の誤差は、誤差の平均がゼロにならない誤差、すなわち、有色性の誤差である。

タイミングＴ４においても、タイミングＴ１において発生したＧＰＳ受信影響が継続しているとする。図４では、タイミングＴ４における実際の車両の位置を、位置ＭＴ４とする。また、図４では、タイミングＴ４において、観測部２１が観測するＧＰＳ車両位置を、位置ＮＴ４とする。

図４では、タイミングＴ４において、タイミングＴ１で発生したＧＰＳ受信影響により、観測部２１が、実際の車両の位置である位置ＭＴ４から距離ｌ４離れた位置ＮＴ４を、ＧＰＳ車両位置として観測する場合を例示する。つまり、図４では、タイミングＴ４において、タイミングＴ１において発生したＧＰＳ受信影響により、観測部２１が観測するＧＰＳ車両位置に対して、実際の車両の位置から距離ｌ４の離間誤差が発生したことを示している。

ＧＰＳ受信部５は、タイミングＴ４におけるＧＰＳ車両位置である位置ＮＴ４を算出する際、タイミングＴ３において算出したＧＰＳ車両位置を加味して算出する。タイミングＴ３において算出したＧＰＳ車両位置は、位置ＮＴ２を加味して算出したものである。したがって、ＧＰＳ受信部５は、タイミングＴ４において、タイミングＴ１で受信したＧＰＳ信号に基づき算出されたＧＰＳ車両位置が反映されたＧＰＳ車両位置を算出する。つまり、観測部２１は、タイミングＴ４において、タイミングＴ１で受信したＧＰＳ信号が反映されたＧＰＳ車両位置を観測する。

また、ＧＰＳ受信部５は、前回算出したＧＰＳ車両位置を加味するため、実際の車両の位置の変化に対して、ＧＰＳ車両位置の変化が乖離することを抑制した位置ＮＴ４を算出する。つまり、ＧＰＳ受信部５は、タイミングＴ１と同じＧＰＳ受信影響がタイミングＴ４においてあるため、仮想直線ＫＴ上に位置するように位置ＮＴ４を算出する。図４では、位置ＮＴ４が仮想直線ＫＴ上に位置する場合を例示するため、離間誤差である距離ｌ３は、距離ｌ１と距離ｌ２と距離ｌ３と同じ値である。

図４のタイミングＴ４において観測部２１が観測するＧＰＳ車両位置の誤差、すなわち、位置ＮＴ４の誤差は、タイミングＴ１で発生したＧＰＳ受信影響により、ＧＰＳ受信影響がない場合のＧＰＳ車両位置の測位誤差と比較して大きい誤差である。

ここで、位置ＮＴ３の誤差と、位置ＮＴ４の誤差とは、相関関係にある。これは、位置ＮＴ３の誤差及び位置ＮＴ４の誤差が、ＧＰＳ受信影響がない場合の誤差と比較して大きい誤差であることから、タイミングＴ１で発生したＧＰＳ受信影響に基づく誤差を含むためである。具体的には、位置ＮＴ３の誤差及び位置ＮＴ４の誤差は、タイミングＴ１で発生したＧＰＳ受信影響に基づく誤差を含むことから、互いに当該ＧＰＳ受信影響に応じた誤差となり、相関関係にある。このように、位置ＮＴ３の誤差及び位置ＮＴ４の誤差は、相関関係にあることから、相関性を有する誤差である。したがって、位置ＮＴ３の誤差及び位置ＮＴ４の誤差は、誤差の平均がゼロにならない誤差、すなわち、有色性の誤差である。

ここまで説明したように、図４では、タイミングＴ１からタイミングＴ４のそれぞれのＧＰＳ車両位置の測位誤差が、タイミングＴ１で発生したＧＰＳ受信影響に基づく誤差を含む場合を示す。したがって、図４において、位置ＮＴ１から位置ＮＴ４のそれぞれの誤差は、互いに相関関係にある。

タイミングＴ５（第２のタイミング）において、タイミングＴ１で発生したＧＰＳ受信影響と異なるＧＰＳ受信影響（第２の影響）が発生したとする。

図４では、タイミングＴ５において、実際の車両の位置を、位置ＭＴ５とする。また、図４では、タイミングＴ５において、観測部２１が観測するＧＰＳ車両位置を、位置ＮＴ５とする。つまり、タイミングＴ５において発生したＧＰＳ信号に基づく影響により、観測部２１は、実際の車両の位置である位置ＭＴ５から距離ｌ５離れた位置ＮＴ５をＧＰＳ車両位置として観測する。これは、タイミングＴ５において発生したＧＰＳ受信影響により、観測部２１が観測するＧＰＳ車両位置に対して、実際の車両の位置から距離ｌ５の離間誤差があることを示す。

図４に示すようにタイミングＴ５における離間誤差は、タイミングＴ１からタイミングＴ４のそれぞれにおける離間誤差と比較して異なる。図４では、離間誤差を示す距離ｌ５は、距離ｌ１から距離ｌ４のいずれの離間誤差よりも短い場合を例示する。つまり、図４では、タイミングＴ５で発生したＧＰＳ受信影響に基づく離間誤差が、タイミングＴ１で発生したＧＰＳ受信影響に基づく離間誤差と比較して小さい場合を例示する。このようにタイミングＴ５における離間誤差が他の離間誤差と異なるのは、タイミングＴ１で発生したＧＰＳ受信影響と異なるＧＰＳ受信影響が、タイミングＴ５において発生したことに起因する。

前述した通り、ＧＰＳ受信部５は、前回算出したＧＰＳ車両位置を加味して、受信したＧＰＳ信号に基づくＧＰＳ車両位置を算出する。そのため、ＧＰＳ受信部５は、タイミングＴ５におけるＧＰＳ車両位置である位置ＮＴ５を算出する際、タイミングＴ４において算出したＧＰＳ車両位置を加味して算出する。タイミングＴ４において算出したＧＰＳ車両位置は、位置ＮＴ３を加味して算出したものであり、タイミングＴ１において受信したＧＰＳ信号が反映されて算出されている。しかしながら、タイミングＴ５において算出したＧＰＳ車両位置は、タイミングＴ１において受信したＧＰＳ信号が反映されないＧＰＳ車両位置と見做すことができる。これは、図４に示すように、タイミングＴ５において観測したＧＰＳ車両位置が、仮想直線ＫＴ上に位置していないためである。このように、タイミングＴ５において仮想直線ＫＴ上に位置していないのは、タイミングＴ５において、タイミングＴ１で発生したＧＰＳ受信影響と異なるＧＰＳ受信影響が発生し、タイミングＴ１からタイミングＴ４で発生した離間誤差が異なることに起因する。したがって、タイミングＴ５において算出したＧＰＳ車両位置は、タイミングＴ１で受信したＧＰＳ信号に基づき算出されたＧＰＳ車両位置が反映されないＧＰＳ車両位置と見做すことができる。つまり、これは、観測部２１が、タイミングＴ５において、タイミングＴ１で受信したＧＰＳ信号が反映されないＧＰＳ車両位置を観測することに相当する。

図４のタイミングＴ５において観測部２１が観測するＧＰＳ車両位置の測位誤差、すなわち、位置ＮＴ５の誤差は、タイミングＴ５で発生したＧＰＳ受信影響により、当該ＧＰＳ受信影響がない場合のＧＰＳ車両位置の測位誤差と比較して大きい誤差である。また、タイミングＴ５において観測部２１が観測するＧＰＳ車両位置の測位誤差は、ＧＰＳ受信影響が異なるため、タイミングＴ１からタイミングＴ４のそれぞれのタイミングにおけるＧＰＳ車両位置の測位誤差と異なる誤差である。したがって、位置ＮＴ５の誤差は、位置ＮＴ１から位置ＮＴ４のそれぞれの誤差と相関関係にない。

図４において、相関時定数とは、以下の期間を示す。すなわち、図４において相関時定数は、タイミングＴ１（第１のタイミング）から、タイミングＴ１で受信したＧＰＳ信号が観測部２１によるＧＰＳ車両位置の観測に反映しないタイミングＴ５（第２のタイミング）までの期間である。より具体的には、図４において相関時定数とは、タイミングＴ１においてＧＰＳ受信影響（第１の影響）が発生し、観測されるＧＰＳ車両位置に対して当該ＧＰＳ受信影響に基づく離間誤差が発生してから、当該ＧＰＳ受信影響と異なるＧＰＳ受信影響（第２の影響）が発生し、この異なるＧＰＳ受信影響に基づく離間誤差がＧＰＳ車両位置に対して発生するまでの期間である。

なお、図４では、相関時定数について、上述した期間に限定されない。例えば、ＧＰＳ受信影響が発生し、当該ＧＰＳ受信影響に基づく実際の車両の位置とＧＰＳ車両位置との離間誤差が発生してから、当該離間誤差が解消するまでの期間でもよい。つまり、図４の場合、この期間は、タイミングＴ１から、タイミングＴ１で発生したＧＰＳ受信影響により離間誤差が解消するまでの期間である。ここで解消とは、離間距離が、タイミングＴ５におけるＧＰＳ受信影響による離間距離となるのではなく、ゼロに近い値となること、すなわち、実際の車両の位置とＧＰＳ車両位置との間の誤差がなくなることを示す。

この相関時定数は、事前のテストやシミュレーション等に基づいて算出され、記憶部等に記憶される。本実施形態では、山道や、高速道路の下に設けられる車道等の種々の車道について算出された、複数の上記に示す期間のうち、最も長い期間が、相関時定数として記憶部等に記憶される。

前述した通り、カルマンゲインＫ_ｋを算出する際、観測量の誤差共分散行列が含む成分のうち、ＧＰＳ車両位置の観測誤差を、式（９）に示すように設定する。つまり、推定部２２は、ＧＰＳ車両位置の測位誤差に√（２τ）を掛けたものをＧＰＳ車両位置の観測誤差として設定する。

式（９）は、以下により算出される。

ＧＰＳ車両位置の誤差が、相関時定数であるτ、及び、ＧＰＳ車両位置の測位誤差であるσの一次マルコフ過程に従うとすると、ＧＰＳ車両位置の誤差の自己相関関数は、式（１０）で表される。自己相関関数とは、本実施形態の場合、時刻に対する、ＧＰＳ車両位置の誤差の自己の相関を示す関数である。

式（１０）は、ＧＰＳ車両位置の誤差の自己相関関数が、相関時定数であるτと、ＧＰＳ車両位置の測位誤差であるσとに基づくエクスポネンシャル関数であることを示す。式（１０）に示すように、時刻ｔが大きければ大きいほど、ＧＰＳ車両位置の誤差の自己の相関は、大きくなる。

ＧＰＳ車両位置の誤差の自己相関関数が式（１０）で表される場合、ウィーナー＝ヒンチンの定理、及び、パワースペクトルと分散との関係に基づき、下記に示す式（１１）が求められる。

式（１１）において、Ｅｒｒ（ｔ）は、ＧＰＳ車両位置の誤差を示す。また、式（１１）において、σ_ｔは、時刻ｔにおけるＧＰＳ車両位置の測位誤差を示す。式（１１）は、式（１１）の左辺を右辺に移項させ、式（１１）の右辺の第１項を左辺に移項させ、τで整理することにより、式（１２）に変形できる。

式（１２）において、右辺の第１項のＥｒｒ（ｔ）／ｄｔは、ＧＰＳ車両位置の誤差の変化量を示す。つまり、式（１２）は、ＧＰＳ車両位置の誤差が、ＧＰＳ車両位置の誤差の変化量に「−τ」を掛けたものと、時刻ｔにおけるＧＰＳ車両位置の測位誤差に「√（２τ）」を掛けたものの和によって表されることを示す。

ここで、式（１２）の右辺の第１項のＥｒｒ（ｔ）／ｄｔは、以下に示す理由により、ゼロと見做すことができる。

ＧＰＳ車両位置の誤差の変化量は、観測や推定等が困難な量である。そのため、ＧＰＳ車両位置の誤差の変化量の期待値は、ゼロであることが考えられる。また、ＧＰＳ受信部５の演算により、ＧＰＳ車両位置は、誤差に変動のない位置、すなわち、誤差が誤差に対する変化量を有さない位置として算出される。このことから、ＧＰＳ車両位置の誤差の変化量は、ゼロであることが考えられる。以上から、ＧＰＳ車両位置の誤差の変化量は、ゼロと見做すことができる。

以上の理由からＧＰＳ車両位置の誤差の変化量をゼロとすると、式（１２）は、右辺の第１項が無くなり、式（１３）で表される。

式（１３）は、時刻ｔにおけるＧＰＳ車両位置の誤差が、時刻ｔにおけるＧＰＳ車両位置の測位誤差に√（２τ）を掛けることによって表記されることを示す。ここで、式（１３）の右辺は、式（９）の右辺に相当し、ＧＰＳ車両位置の観測誤差である。すなわち、ＧＰＳ車両位置の観測誤差は、ＧＰＳ車両の測位誤差に、相関時定数であるτに基づく√（２τ）の重み付けを付与することによって表される。

推定部２２は、カルマンゲインＫ_ｋを算出する際、観測量の誤差共分散行列に含まれる成分のうちＧＰＳ車両位置について、式（１３）に基づいてＧＰＳ車両位置の観測誤差を設定する。つまり、推定部２２は、観測部２１が観測したＧＰＳ車両位置の測位誤差に、√（２τ）の重み付けを付与し、重み付けを付与したＧＰＳ車両位置の測位誤差をＧＰＳ車両位置の観測誤差として、カルマンゲインＫ_ｋの観測量の誤差共分散行列に入力する。より具体的には、推定部２２は、観測量の誤差共分散行列に含まれる成分のうちＧＰＳ車両位置について、式（１４）及び式（１５）に示すようにＧＰＳ車両位置の観測誤差を設定し、カルマンゲインＫ_ｋの観測量の誤差共分散行列に入力する。

式（１４）において、添え字の「ｋ」は、時刻を示す。また、添え字の「ｘＧＰＳ」及び「ｇｘ」は、ｘ軸成分を示す。つまり、式（１４）の右辺は、時刻ｋのｘ軸におけるＧＰＳ車両位置の観測誤差を示す。また、式（１４）の左辺は、√（２τ）に、時刻ｋのｘ軸におけるＧＰＳ車両位置の測位誤差を掛けたもの示す。つまり、ＧＰＳ車両位置の観測誤差のｘ軸成分は、ＧＰＳ車両位置の測位誤差のｘ軸成分に√（２τ）の重み付けが付与され、設定される。

式（１５）において、添え字の「ｋ」は、時刻を示す。また、添え字の「ｙＧＰＳ」及び「ｇｙ」は、ｙ軸成分を示す。つまり、式（１５）の右辺は、時刻ｋのｙ軸におけるＧＰＳ車両位置の観測誤差を示す。また、式（１５）の左辺は、√（２τ）に、時刻ｋのｙ軸におけるＧＰＳ車両位置の測位誤差を掛けたもの示す。つまり、ＧＰＳ車両位置の観測誤差のｙ軸成分は、ＧＰＳ車両位置の測位誤差のｙ軸成分に√（２τ）の重み付けが付与され、設定される。

√（２τ）の重み付けの付与は、τが示す期間において、有色性の誤差であるＧＰＳ車両位置の測位誤差を、まとめて、１のＧＰＳ車両位置の測位誤差（以下、「集ＧＰＳ車両位置測位誤差」と表現する）としてカルマンフィルタで扱うための付与である。前述した通り、τは、相関時定数であり、あるタイミングで受信したＧＰＳ信号が、観測部２１によるＧＰＳ車両位置の観測に反映しない当該あるタイミングと異なる他のタイミングまでの期間である。そして、τは、あるタイミングで発生したＧＰＳ受信影響に基づく有色性の誤差であるＧＰＳ車両位置の測位誤差を含む期間である。推定部２２は、この√（２τ）の重み付けを付与することにより、τの期間において有色性の誤差であるＧＰＳ車両位置の測位誤差をまとめて、集ＧＰＳ車両位置測位誤差としてカルマンフィルタで扱い、カルマンゲインＫ_ｋを算出する。

集ＧＰＳ車両位置測位誤差は、白色性の誤差と見做すことができる。これは、集ＧＰＳ車両位置測位誤差と、τの期間において有色性の誤差であるＧＰＳ車両位置の測位誤差を除く、他のＧＰＳ車両位置の測位誤差と、に相関関係がないためである。例えば、図４の場合、タイミングＴ１からタイミングＴ４までのＧＰＳ車両位置の測位誤差を、まとめて、集ＧＰＳ車両位置測位誤差とした場合、この集ＧＰＳ車両位置測位誤差は、ＧＰＳ受信影響が異なるため、タイミングＴ５のＧＰＳ車両位置の測位誤差と相関関係にない。また、集ＧＰＳ車両位置の測位誤差は、タイミングＴ１の前回のタイミングにおけるＧＰＳ車両位置の測位誤差とも相関関係にない。図４では、タイミングＴ１でＧＰＳ受信影響が発生した場合を例示している。そのため、タイミングＴ１の前回のタイミングでＧＰＳ受信影響があったとしても、このＧＰＳ受信影響は、タイミングＴ１で発生したＧＰＳ受信影響と異なる。したがって、集ＧＰＳ車両位置測位誤差は、タイミングＴ１の前回のタイミングにおけるＧＰＳ車両位置の測位誤差とも相関関係にない。このように、τの期間において有色性の誤差であるＧＰＳ車両位置の測位誤差を、まとめて、集ＧＰＳ車両位置測位誤差とすることにより、この集ＧＰＳ車両位置測位誤差は、白色性の誤差と見做すことができる。

また、集ＧＰＳ車両位置測位誤差は、τの期間において有色性の誤差を示すＧＰＳ車両位置の測位誤差を全て含むＧＰＳ車両位置の測位誤差である。例えば、図４において、タイミングＴ１からタイミングＴ４におけるＧＰＳ車両位置の測位誤差、すなわち、位置ＮＴ１から位置ＮＴ４のそれぞれの誤差を、集ＧＰＳ車両位置測位誤差とした場合、集ＧＰＳ車両位置測位誤差は、位置ＮＴ１から位置ＮＴ４のそれぞれの誤差を含む。したがって、集ＧＰＳ車両位置測位誤差が白色性の誤差として見做せるため、集ＧＰＳ車両位置測位誤差に含まれたＧＰＳ車両位置の測位誤差のそれぞれは、白色性の誤差の一部に相当する。これは、τの期間において有色性の誤差を示すＧＰＳ車両位置の測位誤差をまとめて、集ＧＰＳ車両位置測位誤差とすることにより、τの期間において有色性の誤差を示すＧＰＳ車両位置の測位誤差が、白色性の誤差の一部としてカルマンフィルタで扱われることを示す。すなわち、τの期間において有色性の誤差を示すＧＰＳ車両位置の測位誤差をまとめて、集ＧＰＳ車両位置測位誤差としてカルマンフィルタで扱うことで、τの期間において有色性の誤差を示すＧＰＳ車両位置の測位誤差を、白色性の誤差として見做すことができる。

例えば、図４において、タイミングＴ１からタイミングＴ４におけるＧＰＳ車両位置の測位誤差、すなわち、位置ＮＴ１から位置ＮＴ４のそれぞれの誤差を、集ＧＰＳ車両位置測位誤差とした場合、√（２τ）を付与した位置ＮＴ１から位置ＮＴ４のそれぞれの誤差は、白色性の誤差の一部としてカルマンフィルタで扱われる。このように、位置ＮＴ１から位置ＮＴ４のそれぞれの誤差をまとめて、集ＧＰＳ車両位置測位誤差としてカルマンフィルタで扱うことで、位置ＮＴ１から位置ＮＴ４のそれぞれの誤差を白色性の誤差として見做すことができる。

前述した通り、√（２τ）の重み付けの付与は、τが示す期間において、有色性の誤差であるＧＰＳ車両位置の測位誤差をまとめて、集ＧＰＳ車両位置測位誤差としてカルマンフィルタで扱うための付与である。したがって、√（２τ）の重み付けが付与されたＧＰＳ車両位置の測位誤差、すなわち、式（１３）の左辺は、集ＧＰＳ車両位置測位誤差の一部であり、白色性の誤差であると見做すことができる。つまり、式（１３）のように設定したＧＰＳ車両位置の観測誤差は、白色性の誤差として見做すことができる。これにより、ＧＰＳ車両位置の観測誤差を、式（１３）のように設定することにより、白色性の誤差が前提となる観測量の誤差共分散行列に対してＧＰＳ車両位置の観測誤差を適切に入力でき、推定部２２は、カルマンゲインＫ_ｋを正確に算出できる。そして、推定部２２は、カルマンゲインＫ_ｋが正確に算出されるため、車両の現在地に関する状態を精度よく推定できる。

また、式（１３）のようにＧＰＳ車両位置の観測誤差を設定することにより、推定部２２は、ＧＰＳ信号の受信するたびに、車両の現在地の状態を精度よく推定できる。前述したように、カルマンフィルタにおいて、観測量の誤差共分散行列が含む誤差は、白色性の誤差が前提とされる。そのため、ＧＰＳ受信影響が発生した場合、ＧＰＳ信号を受信するたびに、ＧＰＳ車両位置の測位誤差をそのままＧＰＳ車両位置の観測誤差としてカルマンフィルタに入力すると、車両の現在地の状態の推定の精度が低下する。これに対し、τの期間あけて、ＧＰＳ車両位置の測位誤差をそのままＧＰＳ車両位置の観測誤差としてカルマンフィルタに入力し、車両の現在地の状態の推定することが考えられる。しかしながら、これでは、車両の現在地に関する状態の推定の頻度が低下、また、推定の精度の低下の懸念がある。ここで、式（１３）のようにＧＰＳ車両位置の観測誤差を設定することで、τの期間のＧＰＳ車両位置の測位誤差が有色性の誤差であっても、カルマンフィルタでは白色性の誤差として扱われる。したがって、式（１３）のようにＧＰＳ車両位置の観測誤差を設定することにより、推定部２２は、ＧＰＳ信号の受信するたびに、車両の現在地の状態を精度よく推定できる。

図４では、τが示す期間は、タイミングＴ１においてＧＰＳ受信影響が発生し、観測されるＧＰＳ車両位置に対して、当該ＧＰＳ受信影響に基づく離間誤差が発生してから、当該ＧＰＳ受信影響と異なるＧＰＳ受信影響が発生し、この異なるＧＰＳ受信影響に基づく離間誤差がＧＰＳ車両位置に対して発生するまでの期間である。推定部２２は、この期間に基づく重み付け（√（２τ））の付与により、この期間において有色性の誤差であるＧＰＳ車両位置の測位誤差を、白色性の誤差と見做せるＧＰＳ車両位置の観測誤差として設定できる。そのため、推定部２２は、設定したＧＰＳ車両位置の観測誤差を、観測量の誤差共分散行列（Ｒ_ｋ）に入力することで、カルマンゲインＫ_ｋを正確に算出できる。したがって、推定部２２は、この期間における車両の現在地に関する状態を精度よく推定できる。

また、図４では、τが示す期間は、ＧＰＳ受信影響が発生し、当該ＧＰＳ受信影響に基づく離間誤差がＧＰＳ車両位置に対して発生してから、当該離間誤差が解消するまでの期間でもよい。このような期間であっても、推定部２２は、期間に基づく重み付け（√（２τ））の付与により、このような期間において有色性の誤差であるＧＰＳ車両位置の測位誤差を、白色性の誤差と見做せるＧＰＳ車両位置の観測誤差として設定できる。そのため、推定部２２は、設定したＧＰＳ車両位置の観測誤差を、観測量の誤差共分散行列（Ｒ_ｋ）に入力することで、カルマンゲインＫ_ｋを正確に算出できる。したがって、推定部２２は、ＧＰＳ受信影響が発生し、当該ＧＰＳ受信影響に基づく離間誤差がＧＰＳ車両位置に対して発生してから、当該離間誤差が解消するまでの期間における車両の現在地に関する状態を、精度よく推定できる。

なお、ナビゲーション装置１がＧＰＳ受信影響の有無を判別可能な構成である場合において、ＧＰＳ受信影響であると判別した場合、ＧＰＳ車両位置の観測誤差を式（１６）及び式（１７）のように設定してもよい。

ＶＡＲ_ｒｘは、時刻ｋにおける、ＧＰＳ車両位置のｘ軸成分と、予測処理で予想した車両の位置（車両状態予測値における車両の位置）のｘ軸成分との分散を示す。つまり、式（１６）は、式（１４）に当該分散を加えてＧＰＳ車両位置の観測誤差を設定する。

ＶＡＲ_ｒｙは、時刻ｋにおける、ＧＰＳ車両位置のｙ軸成分と、予測処理で予想した車両の位置（車両状態予測値の車両の位置）のｙ軸成分との分散を示す。つまり、式（１７）は、式（１５）に当該分散を加えてＧＰＳ車両位置の観測誤差を設定する。

このように、ＧＰＳ受信影響の有無の判定が可能に構成され、ＧＰＳ受信影響がある場合、ＧＰＳ車両位置の観測誤差を式（１６）及び式（１７）に示すように、設定する。これにより、ＧＰＳ車両位置と予測処理で予測した車両の位置との分散を加えることで、ＧＰＳ車両位置の観測誤差がこれら位置に基づく分散を含むため、より正確にＧＰＳ車両位置の観測誤差が算出される。そのため、カルマンゲインＫ_ｋがより正確に算出され、推定部２２は、車両の現在地に関する状態を精度よく推定できる。

なお、式（１６）及び式（１７）は、ＧＰＳ受信影響の有無の判定が可能に構成され、ＧＰＳ受信影響がある場合に、ＧＰＳ車両位置の観測誤差を設定する式である。しかしながら、式（１６）及び式（１７）は、ＧＰＳ受信影響の有無に関係なく用いられてもよい。これは、ＧＰＳ受信影響がない場合、式（１６）のＶＡＲ_ｒｘと式（１７）のＶＡＲ_ｒｙは、０に近似することが考えられる。したがって、式（１６）及び式（１７）を、ＧＰＳ受信影響の有無に関係なく、ＧＰＳ車両位置の観測誤差の設定に用いてもよい。この場合でも、上述した効果を奏する。

前述した通り、ナビゲーション装置１の制御部２は、推定した車両の状態に基づき、マップマッチングの対象となるリンクの評価を実行する。

図５は、推定した車両の状態に基づくマップマッチングの処理を説明するため、地図上の道路Ｒ１と、道路Ｒ２とを示す図である。図５において、道路Ｒ１は、方向Ｙ１に向かって延在する道路である。また、道路Ｒ２は、方向Ｙ１と並行でない方向Ｙ２に向かって延在する道路である。

図５において、リンクＬ１は、道路Ｒ１に対応するリンクである。また、リンクＬ２は、道路Ｒ２に対応するリンクである。

また、図５において、マークαは、推定部２２により推定された車両の位置を示すマークである。以下、図５を用いた説明では、推定部２２により、車両が、位置Ｍ１に位置し、進行方向Ｘ１に向かって走行しているものと推定されたとする。図５に示すように、位置Ｍ１は、道路Ｒ１上の位置であって、道路Ｒ１の幅方向の中心から、方向Ｙ１に向かって右側に離間した位置である。

道路Ｒ１と、道路Ｒ２と、推定された車両の位置との関係が、図５に示す関係である場合、マップマッチングに際し、まず、制御部２は、地図データ３ａを参照し、マップマッチングの候補となるリンクであるマップマッチング候補リンクを取得する。制御部２は、マップマッチング候補リンクとして、推定された車両の位置から予め設定された所定の範囲内に位置し、かつ、車両とリンクとの方位の誤差が所定範囲内である１又は複数のリンクを取得する。図５の例では、リンクＬ１、及び、リンクＬ２は、推定された車両の位置である位置Ｍ１から所定範囲に位置し、また、車両とリンクとの方位の誤差が所定範囲内のリンクである。このため、制御部２は、リンクＬ１、及び、リンクＬ２をマップマッチング候補リンクとして取得する。

次いで、制御部２は、取得したマップマッチング候補リンクのそれぞれについて、リンクを評価する評価量を算出する。

評価量は、推定された車両の位置とマップマッチング候補リンクの位置との誤差、及び、推定された車両の方位とマップマッチング候補リンクの方位との誤差に基づいて、下記に示す式（１８）により算出される値である。

τ＝δｘ^２／Δｘ^２＋δｙ^２／Δｙ^２＋δθ^２／Δθ^２・・・（１８）

ここで、推定された車両の位置とマップマッチング候補リンクの位置との誤差とは、推定された車両の位置からマップマッチング候補リンクに対して垂線を延ばした場合における垂線及びマップマッチング候補リンクとの交点と、推定された車両の位置との、ｘ軸方向、及び、ｙ軸方向における差である。式（１８）において、δｘは、当該交点のｘ座標と、推定された車両の位置のｘ座標との差を示す。また、δｙは、当該交点のｙ座標と、推定された車両の位置のｙ座標との差を示す。

図５の例において、推定された車両の位置とリンクＬ１との位置の誤差は、位置Ｍ１からリンクＬ１に対して延ばした垂線Ｓ１及びリンクＬ１の交点ＭＭ１と、位置Ｍ１とのｘ軸方向、及び、ｙ軸方向における差である。また、図５において、推定された車両の位置とリンクＬ２との位置の誤差は、位置Ｍ１からリンクＬ２に対して延ばした垂線Ｓ２及びリンクＬ２の交点ＭＭ２と、位置Ｍ１とのｘ軸方向、及び、ｙ軸方向における差である。

また、推定された車両の方位とマップマッチング候補リンクの方位との誤差とは、推定された車両の方位に対応する角度と、マップマッチング候補リンクの方位に対応する角度との差である。

前述した通り、車両の方位とは、車両の進行方向の方角を意味する。図５の例では、推定された車両の方位は、進行方向Ｘ１の方角である。また、車両の方位に対応する角度とは、東に向かう方向を基準として、東に向かう方向と、車両の方位との反時計回りの離間角度を意味する。図５の例では、推定された車両の方位に対応する角度は、仮想直線Ｋ１が東西に延びる仮想直線であるとした場合に、角度θ１である。

また、リンクの方位とは、リンクが延在する方向の方角を意味する。リンクが延在する方向とは、リンクに沿った２つの方向のうち、リンクに対応する道路において車両が走行可能な方向に対応する方向を意味する。また、リンクの方位に対応する角度とは、東に向かう方向を基準として、東に向かう方向と、リンクの方位との反時計回りの離間角度を意味する。

図５の例では、リンクＬ１の方位は、方向Ｚ１の方角である。また、リンクＬ１の方位に対応する角度は、仮想直線Ｋ２が東西に延びる仮想直線であるとした場合に、角度θ２である。また、図５の例では、リンクＬ２の方位は、方向Ｚ２の方角である。また、リンクＬ２の方位に対応する角度は、仮想直線Ｋ３が東西に延びる仮想直線であるとした場合に、角度θ３である。

図５の例において、車両とリンクＬ１との方位誤差は、角度θ１と角度θ２との差である。また、車両とリンクＬ２との方位誤差は、角度θ１と角度θ３との差である。つまり、図５の例において、式（１８）では、δθは、角度θ１と角度θ２との差、または、角度θ１と角度θ３との差を示す。

式（１８）に示すように、評価量τは、δｘを二乗した値にΔｘを二乗した値で割ったものと、δｙを二乗した値にΔｙを二乗した値で割ったものと、δθを二乗した値にΔθを二乗した値で割ったものと、の和により算出される。Δｘは、推定部２２が推定した車両のｘ軸方向の位置の誤差を示す。つまり、Δｘ^２は、推定部２２が推定した車両のｘ軸方向の位置の分散を示す。また、Δｙは、推定部２２が推定した車両のｙ軸方向の位置の誤差を示す。つまり、Δｙ^２は、推定部２２が推定した車両のｙ軸方向の位置の分散を示す。また、Δθは、推定部２２が推定した車両の方位の誤差を示す。つまり、Δθ^２は、推定部２２が推定した車両の方位の分散を示す。Δｘ^２、Δｙ^２、及び、Δθ^２は、推定部２２が推定した車両状態推定値の誤差共分散行列が含む分散である。このように、評価量は、車両の位置とリンクの位置との誤差を推定した車両の位置の誤差により無次元化した値と、車両の方位とリンクの方位との誤差を、推定した車両の位置の誤差により無次元化した値との和により算出される値である。

図５の例では、制御部２は、マップマッチング候補リンクとして取得したリンクＬ１の評価量とリンクＬ２の評価量とを算出する。

リンクＬ１の評価量をτ１とし、位置Ｍ１と交点ＭＭ１とのｘ座標の差をδｘ１とし、位置Ｍ１と交点ＭＭ１とのｙ座標の差をδｙ１とし、角度θ１と角度θ２との差をδθ１とした場合、評価量τ１は、下記に示す式（１９）で表される。

τ１＝δｘ１^２／Δｘ^２＋δｙ１^２／Δｙ^２＋δθ１^２／Δθ^２・・・（１９）

一方、リンクＬ２の評価量をτ２とし、位置Ｍ１と交点ＭＭ２とのｘ座標の差をδｘ２とし、位置Ｍ１と交点ＭＭ２とのｙ座標の差をδｙ２とし、角度θ１と角度θ３との差をδθ２とした場合、評価量τ２は、下記に示す式（２０）で表される。

τ２＝δｘ２^２／Δｘ^２＋δｙ２^２／Δｙ^２＋δθ２^２／Δθ^２・・・（２０）

図５の例では、距離ｌ２より距離ｌ１が小さい。すなわち、位置Ｍ１と交点ＭＭ２とにおけるｘ軸座標及びｙ座標の差より、位置Ｍ１と交点ＭＭ２とにおけるｘ座標及びｙ座標の差が小さい。また、図５の例では、角度θ１と角度θ３との差より角度θ１と角度θ２との差が小さい。このため、リンクＬ１の評価量であるτ１の値の方が、リンクＬ２の評価量であるτ２の値よりも小さくなる。したがって、制御部２は、評価量が小さいリンクＬ１を、車両の現在位置を対応付けるリンクとして決定する。

このように、制御部２は、推定した車両の状態に基づいて、マップマッチングの対象となるリンクの評価を実行する。上述したように、制御部２は、リンクを評価する際、推定部２２が推定した車両状態推定値の誤差共分散行列が含む分散に基づき、リンクを評価する。当該誤差共分散行列は、白色性の誤差としてＧＰＳ車両位置の観測誤差が入力されたカルマンゲインＫ_ｋに基づいて、算出された行列であり、精度よく算出された誤差を含む行列である。したがって、制御部２は、当該誤差共分散行列が含む分散をリンクの評価に用いることにより、正確にリンクを評価できる。

以上、説明したように、ナビゲーション装置１（位置推定装置、車載装置）は、ＧＰＳ信号を受信するＧＰＳ受信部５と、ＧＰＳ受信部５が受信したＧＰＳ信号に基づくＧＰＳ車両位置（ＧＰＳ測位位置）を含む観測量を観測する観測部２１と、観測部２１が観測する観測量とカルマンフィルタとに基づいて、車両の現在地に関する状態量を推定する推定部２２と、を備える。推定部２２は、車両状態予測値（状態量の予測値）と、車両状態予測値の誤差共分散行列（予測値の誤差）とを算出する。また、推定部２２は、車両状態予測値と、車両状態予測値の誤差共分散行列と、観測部２１が観測する観測量の誤差共分散行列（観測量の誤差）とに基づいて、車両状態推定値（状態量の推定値）と、車両状態推定値の誤差共分散行列（推定値の誤差）とを算出する。そして、推定部２２は、車両状態推定値と、車両状態推定値の誤差共分散行列とを算出する際、あるタイミング（第１のタイミング）から、当該あるタイミングにおいて受信したＧＰＳ信号が、観測部２１による前記ＧＰＳ車両位置の観測に反映しない当該あるタイミングと異なる他のタイミング（第２のタイミング）までの期間に基づく重み付けを、ＧＰＳ車両位置の測位誤差に付与する。

ＧＰＳ車両位置の測位誤差に当該期間に基づく重み付けを付与することにより、ＧＰＳ車両位置の測位誤差は、白色性の誤差として見做せるＧＰＳ車両位置の観測誤差として設定される。したがって、白色性の誤差が前提となる観測量の誤差共分散行列に対してＧＰＳ車両位置の観測誤差を適切に入力でき、推定部２２は、カルマンゲインＫ_ｋを正確に算出できる。そして、推定部２２は、カルマンゲインＫ_ｋが正確に算出されるため、車両の現在地に関する状態を精度よく推定できる。

また、推定部２２は、車両状態推定値と、車両状態推定値の誤差共分散行列を算出する際、あるタイミングで発生したＧＰＳ受信影響（第１の影響）に基づく誤差（離間誤差）がＧＰＳ車両位置に対して発生してから、あるタイミングと異なる他のタイミングで発生した当該ＧＰＳ受信影響と異なるＧＰＳ受信影響（第２の影響）に基づく誤差（離間誤差）がＧＰＳ車両位置に対して発生するまでの期間に基づく重み付けを、ＧＰＳ車両位置の測位誤差に付与する。

当該期間は、図４の場合、タイミングＴ１においてＧＰＳ受信影響が発生し、観測されるＧＰＳ車両位置に対して、当該ＧＰＳ受信影響に基づく離間誤差が発生してから、タイミングＴ５において当該ＧＰＳ受信影響と異なるＧＰＳ受信影響が発生し、この異なるＧＰＳ受信影響に基づく離間誤差がＧＰＳ車両位置に対して発生するまでの期間である。推定部２２は、カルマンゲインＫ_ｋを算出する際、この期間に基づく重み付け（√（２τ））の付与により、この期間において有色性の誤差であるＧＰＳ車両位置の測位誤差を、白色性の誤差と見做せるＧＰＳ車両位置の観測誤差として観測量の誤差共分散行列（Ｒ_ｋ）に入力し、カルマンゲインＫ_ｋを算出する。したがって、推定部２２は、この期間における車両の現在地に関する状態を精度よく推定できる。

また、推定部２２は、車両状態推定値と、車両状態推定値の誤差共分散行列を算出する際、ＧＰＳ受信影響に基づく誤差（離間誤差）がＧＰＳ車両位置に対して発生してから解消するまでの期間に基づく重み付けを、ＧＰＳ車両位置の誤差に付与する。

前述した通り、τが示す期間は、ＧＰＳ受信影響が発生し、当該ＧＰＳ受信影響に基づく離間誤差がＧＰＳ車両位置に対して発生してから、当該離間誤差が解消するまでの期間でもよい。推定部２２は、このような期間であっても、この期間において有色性の誤差であるＧＰＳ車両位置の測位誤差を、白色性の誤差として見做せるＧＰＳ車両位置の観測誤差として観測量の誤差共分散行列（Ｒ_ｋ）に入力し、カルマンゲインＫ_ｋを算出する。したがって、推定部２２は、この期間における車両の現在地に関する状態を精度よく推定できる。

上述した実施形態は、あくまでも本発明の一態様を例示するものであって、本発明の趣旨を逸脱しない範囲で任意に変形、及び応用が可能である。

例えば、図１は、本願発明を理解容易にするために、ナビゲーション装置１の機能構成を主な処理内容に応じて分類して示した概略図であり、ナビゲーション装置１の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、１つの構成要素がさらに多くの処理を実行するように分類することもできる。

また、例えば、図２、及び、図３のフローチャートの処理単位は、制御部２の処理を理解容易にするために、主な処理内容に応じて分割したものであり、処理単位の分割の仕方や名称によって、本発明が限定されることはない。制御部２の処理は、処理内容に応じて、さらに多くの処理単位に分割してもよい。また、１つの処理単位がさらに多くの処理を含むように分割してもよい。

また、例えば、上述した実施形態では、位置推定装置を、車両に搭載される車載装置であるナビゲーション装置１として例示したが、位置推定装置の形態は任意であり、例えば歩行者が携帯するポータブル型の装置でも良い。

１ ナビゲーション装置（位置推定装置、車載装置）
５ ＧＰＳ受信部
２１ 観測部
２２ 推定部

Claims (6)

1. ＧＰＳ信号を受信するＧＰＳ受信部と、
   前記ＧＰＳ受信部が受信した前記ＧＰＳ信号に基づくＧＰＳ測位位置を含む観測量を観測する観測部と、
   前記観測部が観測する前記観測量とカルマンフィルタとに基づいて、現在地に関する状態量を推定する推定部と、を備え、
   前記推定部は、
   前記状態量の予測値と、前記予測値の誤差とを算出し、
   前記予測値と、前記予測値の誤差と、前記観測部が観測する前記観測量の誤差とに基づいて、前記状態量の推定値と、前記推定値の誤差とを算出し、
   前記推定値と、前記推定値の誤差とを算出する際、
   前記ＧＰＳ信号の受信に係る第１の影響に基づく誤差が前記ＧＰＳ測位位置に対して発生してから、前記第１の影響と異なる第２の影響に基づく誤差が前記ＧＰＳ測位位置に対して発生するまでの期間に基づく重み付けを、前記ＧＰＳ測位位置の誤差に付与する、
   ことを特徴とする位置推定装置。
2. ＧＰＳ信号を受信するＧＰＳ受信部と、
   前記ＧＰＳ受信部が受信した前記ＧＰＳ信号に基づくＧＰＳ測位位置を含む観測量を観測する観測部と、
   前記観測部が観測する前記観測量とカルマンフィルタとに基づいて、現在地に関する状態量を推定する推定部と、を備え、
   前記推定部は、
   前記状態量の予測値と、前記予測値の誤差とを算出し、
   前記予測値と、前記予測値の誤差と、前記観測部が観測する前記観測量の誤差とに基づいて、前記状態量の推定値と、前記推定値の誤差とを算出し、
   前記推定値と、前記推定値の誤差とを算出する際、
   前記ＧＰＳ信号の受信に係る影響に基づく誤差が前記ＧＰＳ測位位置に対して発生してから解消するまでの期間に基づく重み付けを、前記ＧＰＳ測位位置の誤差に付与する、
   ことを特徴とする位置推定装置。
3. 車両に搭載される車載装置であり、前記車両の現在地に関する前記状態量を推定する
   ことを特徴とする請求項１又は２に記載の位置推定装置。
4. ＧＰＳ信号に基づくＧＰＳ測位位置を含む観測量とカルマンフィルタとに基づいて、現在地に関する状態量を推定する推定方法であって、
   前記状態量の予測値と、前記予測値の誤差とを算出し、
   前記予測値と、前記予測値の誤差と、前記観測量の誤差とに基づいて、前記状態量の推定値と、前記推定値の誤差とを算出し、
   前記推定値と、前記推定値の誤差とを算出する際、前記ＧＰＳ信号の受信に係る第１の影響に基づく誤差が前記ＧＰＳ測位位置に発生してから、前記第１の影響と異なる第２の影響に基づく誤差が前記ＧＰＳ測位位置に発生するまでの期間に基づく重み付けを、前記ＧＰＳ測位位置の誤差に付与する、
   ことを特徴とする推定方法。
5. ＧＰＳ信号に基づくＧＰＳ測位位置を含む観測量とカルマンフィルタとに基づいて、現在地に関する状態量を推定する推定方法であって、
   前記状態量の予測値と、前記予測値の誤差とを算出し、
   前記予測値と、前記予測値の誤差と、前記観測量の誤差とに基づいて、前記状態量の推定値と、前記推定値の誤差とを算出し、
   前記推定値と、前記推定値の誤差を算出する際、前記ＧＰＳ信号の受信に係る影響に基づく誤差が前記ＧＰＳ測位位置に発生してから解消するまでの期間に基づく重み付けを、前記ＧＰＳ測位位置の誤差に付与する、
   ことを特徴とする推定方法。
6. 車両の現在地に関する状態量を推定する
   ことを特徴とする請求項４又は５に記載の推定方法。

Images