特開2017-194302(P2017-194302A)IP Force 特許公報掲載プロジェクト 2015.5.11 β版

知財求人 - 知財ポータルサイト「IP Force」

▶ クラリオン株式会社の特許一覧
<>
  • 特開2017194302-位置推定装置、及び、推定方法 図000020
  • 特開2017194302-位置推定装置、及び、推定方法 図000021
  • 特開2017194302-位置推定装置、及び、推定方法 図000022
  • 特開2017194302-位置推定装置、及び、推定方法 図000023
  • 特開2017194302-位置推定装置、及び、推定方法 図000024
< >
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】特開2017-194302(P2017-194302A)
(43)【公開日】2017年10月26日
(54)【発明の名称】位置推定装置、及び、推定方法
(51)【国際特許分類】
   G01C 21/28 20060101AFI20170929BHJP
   G01S 19/47 20100101ALI20170929BHJP
【FI】
   G01C21/28
   G01S19/47
【審査請求】未請求
【請求項の数】8
【出願形態】OL
【全頁数】30
(21)【出願番号】特願2016-83340(P2016-83340)
(22)【出願日】2016年4月19日
(71)【出願人】
【識別番号】000001487
【氏名又は名称】クラリオン株式会社
(74)【代理人】
【識別番号】110001081
【氏名又は名称】特許業務法人クシブチ国際特許事務所
(72)【発明者】
【氏名】岡田 和弘
【テーマコード(参考)】
2F129
5J062
【Fターム(参考)】
2F129AA03
2F129BB03
2F129BB20
2F129BB22
2F129BB26
2F129BB33
2F129BB48
2F129BB49
2F129HH02
2F129HH12
2F129HH20
5J062AA09
5J062BB01
5J062CC07
5J062FF04
(57)【要約】
【課題】カルマンフィルタにより現在地に関する状態を精度よく推定できるようにすることを目的とする。
【解決手段】ナビゲーション装置1は、GPS信号を受信するGPS受信部5と、受信したGPS信号に基づくGPS車両位置を含む観測量を観測する観測部21と、観測部21が観測する観測量とカルマンフィルタとに基づいて、現在地に関する状態量を推定する推定部22と、を備え、推定部22は、状態量の予測値と、予測値の誤差とを算出し、予測値と、予測値の誤差と、観測部21が観測する観測量の誤差とに基づいて、状態量の推定値と、推定値の誤差とを算出し、推定値と、推定値の誤差とを算出する際、第1のタイミングから、第1のタイミングにおいて受信したGPS信号が、観測部21によるGPS車両位置の観測に反映しない第2のタイミングまでの期間に基づく重み付けを、GPS車両位置の誤差に付与する。
【選択図】図1
【特許請求の範囲】
【請求項1】
GPS信号を受信するGPS受信部と、
前記GPS受信部が受信した前記GPS信号に基づくGPS測位位置を含む観測量を観測する観測部と、
前記観測部が観測する前記観測量とカルマンフィルタとに基づいて、現在地に関する状態量を推定する推定部と、を備え、
前記推定部は、
前記状態量の予測値と、前記予測値の誤差とを算出し、
前記予測値と、前記予測値の誤差と、前記観測部が観測する前記観測量の誤差とに基づいて、前記状態量の推定値と、前記推定値の誤差とを算出し、
前記推定値と、前記推定値の誤差とを算出する際、
第1のタイミングから、前記第1のタイミングにおいて受信した前記GPS信号が、前記観測部による前記GPS測位位置の観測に反映しない第2のタイミングまでの期間に基づく重み付けを、前記GPS測位位置の誤差に付与する、
ことを特徴とする位置推定装置。
【請求項2】
前記推定部は、
前記推定値と、前記推定値の誤差を算出する際、
前記GPS信号の受信に係る第1の影響に基づく誤差が前記GPS測位位置に対して発生してから、前記第1の影響と異なる第2の影響に基づく誤差が前記GPS測位位置に対して発生するまでの期間に基づく重み付けを、前記GPS測位位置の誤差に付与する、
ことを特徴とする請求項1に記載の位置推定装置。
【請求項3】
前記推定部は、
前記推定値と、前記推定値の誤差を算出する際、
前記GPS信号の受信に係る影響に基づく誤差が前記GPS測位位置に対して発生してから解消するまでの期間に基づく重み付けを、前記GPS測位位置の誤差に付与する、
ことを特徴とする請求項1に記載の位置推定装置。
【請求項4】
車両に搭載される車載装置であり、前記車両の現在地に関する前記状態量を推定する
ことを特徴とする請求項1から3に記載の位置推定装置。
【請求項5】
GPS信号に基づくGPS測位位置を含む観測量とカルマンフィルタとに基づいて、現在地に関する状態量を推定する推定方法であって、
前記状態量の予測値と、前記予測値の誤差とを算出し、
前記予測値と、前記予測値の誤差と、前記観測量の誤差とに基づいて、前記状態量の推定値と、前記推定値の誤差とを算出し、
前記推定値と、前記推定値の誤差とを算出する際、第1のタイミングから、前記第1のタイミングにおいて受信した前記GPS信号が、前記GPS測位位置の観測に反映しない第2のタイミングまでの期間に基づく重み付けを、前記GPS測位位置の誤差に付与する、
ことを特徴とする推定方法。
【請求項6】
前記推定値と、前記推定値の誤差を算出する際、前記GPS信号の受信に係る第1の影響に基づく誤差が前記GPS測位位置に発生してから、前記第1の影響と異なる第2の影響に基づく誤差が前記GPS測位位置に発生するまでの期間に基づく重み付けを、前記GPS測位位置の誤差に付与する、
ことを特徴とする請求項5に記載の推定方法。
【請求項7】
前記推定値と、前記推定値の誤差を算出する際、前記GPS信号の受信に係る影響に基づく誤差が前記GPS測位位置に発生してから解消するまでの期間に基づく重み付けを、前記GPS測位位置の誤差に付与する、
ことを特徴とする請求項5に記載の推定方法。
【請求項8】
車両の現在地に関する状態量を推定する
ことを特徴とする請求項5から7に記載の推定方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、位置推定装置、及び、推定方法に関する。
【背景技術】
【0002】
本技術分野の背景技術として、特開2012−215491号公報(特許文献1)がある。特許文献1には、「GPSユニット3とINS(登録商標)ユニット5とを備えた位置算出装置1において、位置算出精度判定部6は、位置算出精度を判定する。影響度合設定部7は、位置算出精度判定部6により判定された位置算出精度に基づいて、GPS計測結果のINS計測結果に対する影響度合を設定する。カップリング処理部9は、影響度合設定部7により設定された影響度合に基づいてGPS計測結果とINS計測結果とのカップリング処理を実行して、移動体の位置を算出する。」と記載されている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開2012−215491号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
一般に、カルマンフィルタに基づいて位置や方位等の現在地に関する状態を推定する際、センサーやGPS信号等に基づき観測される位置や方位等の観測量の誤差は、誤差の平均がゼロとなる白色性の誤差であることが前提とされる。しかしながら、GPS信号に基づき観測される位置の誤差は、誤差の平均がゼロでない有色性の誤差である場合がある。特許文献1では、GPS信号に基づき観測される位置の誤差が有色性の誤差であることを考慮しておらず、カルマンフィルタにより現在地に関する状態を精度よく推定できない。
そこで、本発明は、カルマンフィルタにより現在地に関する状態を精度よく推定できるようにすることを目的とする。
【課題を解決するための手段】
【0005】
上記目的を達成するために、位置推定装置は、GPS信号を受信するGPS受信部と、前記GPS受信部が受信した前記GPS信号に基づくGPS測位位置を含む観測量を観測する観測部と、前記観測部が観測する前記観測量とカルマンフィルタとに基づいて、現在地に関する状態量を推定する推定部と、を備え、前記推定部は、前記状態量の予測値と、前記予測値の誤差とを算出し、前記予測値と、前記予測値の誤差と、前記観測部が観測する前記観測量の誤差とに基づいて、前記状態量の推定値と、前記推定値の誤差とを算出し、前記推定値と、前記推定値の誤差とを算出する際、第1のタイミングから、前記第1のタイミングにおいて受信した前記GPS信号が、前記観測部による前記GPS測位位置の観測に反映しない第2のタイミングまでの期間に基づく重み付けを、前記GPS測位位置の誤差に付与する、ことを特徴とする。
【発明の効果】
【0006】
本発明によれば、カルマンフィルタにより現在地に関する状態を精度よく推定できる。
【図面の簡単な説明】
【0007】
図1】ナビゲーション装置の構成を示すブロック図である。
図2】ナビゲーション装置の動作を示すフローチャートである。
図3】車両状態推定処理における推定部の動作を示すフローチャートである。
図4】相関時定数を説明するための図である。
図5】推定した車両の状態に基づくマップマッチングの処理を説明するための図である。
【発明を実施するための形態】
【0008】
図1は、ナビゲーション装置1(位置推定装置、車載装置)の構成を示すブロック図である。
【0009】
ナビゲーション装置1は、車両に搭載される車載装置であり、車両に搭乗しているユーザの操作に従って、地図の表示や、地図における車両の現在位置の表示、目的地までの経路探索、経路案内等を実行する。なお、ナビゲーション装置1は、車両のダッシュボード等に固定されてもよいし、車両に対し着脱可能なものであっても良い。
【0010】
図1に示すように、ナビゲーション装置1は、制御部2と、記憶部3と、タッチパネル4と、GPS受信部5と、車速センサー6と、ジャイロセンサー7と、加速度センサー8とを備える。
【0011】
制御部2は、CPUや、ROM、RAM、その他の制御回路等を備え、ナビゲーション装置1の各部を制御する。制御部2は、ROMや記憶部3等に記憶された制御プログラムを実行することにより、後述する観測部21、及び、推定部22として機能する。
【0012】
記憶部3は、ハードディスクや、EEPROM等の不揮発性メモリを備え、データを書き換え可能に記憶する。記憶部3は、制御部2が実行する制御プログラムのほか、地図データ3aを記憶する。地図データ3aは、交差点やその他の道路網上の接続点を示すノードに関するノード情報や、ノードとノードとの道路区間を示すリンクに関するリンク情報、地図の表示に係る情報等を有する。リンク情報は、リンク毎に、少なくとも、リンクの位置に関する情報と、リンクの方位に関する情報とを含む。
【0013】
タッチパネル4は、表示パネル4aと、タッチセンサー4bとを備える。表示パネル4aは、液晶ディスプレイやEL(Electro Luminescent)ディスプレイ等により構成され、制御部2の制御の下、各種情報を表示パネル4aに表示する。タッチセンサー4bは、表示パネル4aに重ねて配置され、ユーザのタッチ操作を検出し、タッチ操作された位置を示す信号を制御部2に出力する。制御部2は、タッチセンサー4bからの入力に基づいて、タッチ操作に対応する処理を実行する。
【0014】
GPS受信部5は、GPSアンテナ5aを介してGPS衛星から送信されるGPS信号を受信する。そして、GPS受信部5は、受信したGPS信号に基づいて、車両とGPS衛星間の距離及び距離の変化率を所定数以上の衛星に対して測定することにより、少なくとも車両の位置(GPS測位位置)と車両の進行方向の方角(以下、「車両の方位」と表現する)を算出する。また、GPS受信部5は、車両の位置、及び、車両の方位を算出する際、受信したGPS信号と、前回算出した、車両の位置、及び、車両の方位を加味して算出する。以下の説明において、GPS信号に基づく車両の位置を、GPS車両位置と表現する。GPS受信部5は、GPS車両位置と、車両の方位とを算出する際、GPS車両位置の誤差と、車両の方位の誤差とを算出する。GPS受信部5は、GPS車両位置を示す情報、GPS車両位置の誤差を示す情報、車両の方位を示す情報、及び、車両の方位の誤差を示す情報を、制御部2に出力する。
【0015】
車速センサー6は、車両の車速を検出して、検出した車速を示す信号を制御部2に出力する。
【0016】
ジャイロセンサー7は、例えば振動ジャイロにより構成され、車両の回転による角速度を検出する。ジャイロセンサー7は、検出した角速度を示す信号を制御部2に出力する。
【0017】
加速度センサー8は、車両に作用する加速度(例えば、進行方向に対する車両の傾き)を検出する。加速度センサー8は、検出した加速度を示す信号を制御部2に出力する。
【0018】
図1に示すように、制御部2は、観測部21と、推定部22とを備える。
【0019】
観測部21は、車速センサー6、ジャイロセンサー7、及び、加速度センサー8から出力される信号と、GPS受信部5から出力される情報とに基づき、車両の変動に関する観測量を観測する。
【0020】
観測部21は、車速センサー6から出力される車速を示す信号に基づき、観測量として車両の速度を観測する。観測部21は、所定の演算により、車速を示す信号から、車両の速度と車両の速度の誤差とを算出する。
【0021】
また、観測部21は、ジャイロセンサー7から出力される角速度を示す信号に基づき、車両の回転による角速度を観測量として観測する。観測部21は、所定の演算により、角速度を示す信号から、車両の角速度と車両の角速度の誤差とを算出する。
【0022】
また、観測部21は、加速度センサー8から出力される加速度を示す信号に基づき、観測量として車両の加速度を観測する。観測部21は、所定の演算により、加速度を示す信号から、車両の加速度と車両の加速度の誤差とを算出する。
【0023】
また、観測部21は、GPS受信部5から出力されたGPS車両位置を示す情報と車両の方位を示す情報とに基づき、GPS車両位置と車両の方位とを観測量として観測する。
【0024】
推定部22は、観測部21が観測する車両の速度、車両の加速度、車両の角速度、車両の位置、及び、車両の方位に基づき、カルマンフィルタにより、車両の現在地に関する状態を示す状態量を推定する。本実施形態では、推定部22は、車両の速度、車両の角速度、車両の位置、及び、車両の方位を、車両の状態量として推定する。
【0025】
後述するが、制御部2は、推定部22が推定した車両の状態量に基づき、マップマッチングの対象となるリンクの評価を実行する。
【0026】
なお、推定部22が推定する車両の速度は、車両の状態量に相当する。また、観測部21が観測する車両の速度は、観測量に相当する。同様に、推定部22が推定する車両の角速度は、車両の状態量に相当する。また、観測部21が観測する車両の角速度は、観測量に相当する。
【0027】
ここで、カルマンフィルタによる基本的な車両の状態量の推定について説明する。
【0028】
本実施形態では、カルマンフィルタにより推定する車両の状態量は、車両の位置、車両の方位、車両の速度、及び、車両の角速度である。以下に、カルマンフィルタにより推定する車両の各状態量を示す。
【0029】
x:車両の位置のx座標
y:車両の位置のy座標
θ:車両の方位
v:車両の速度
ω:車両の角速度
【0030】
ここで、車両の状態量をベクトル表記した状態ベクトルを、(x、y、θ、v、ω)とすると、車両の状態量についての状態方程式は、式(1)で表される。
【0031】
【数1】
【0032】
添え字のk+1及びkは、時刻を示す。例えば、式(1)の左辺である(xk+1、yk+1、θk+1、vk+1、ωk+1)は、時刻k+1における車両の状態量を示す。式(1)において、右辺の第2項であるqは、システム雑音(平均0、誤差共分散行列であるQを有する正規分布N(0、Q))である。誤差共分散行列とは、分散と共分散との行列である。
【0033】
前述した通り、観測部21は、観測量として、車両の速度、車両の角速度、車両の加速度、GPS車両位置、及び、車両の方位を観測する。前述した通り、観測部21は、車両の速度について、車速センサー6からの出力に基づき観測する。また、観測部21は、車両の角速度について、ジャイロセンサー7からの出力に基づき観測する。また、観測部21は、車両の加速度について、加速度センサー8からの出力に基づき観測する。また、観測部21は、GPS受信部5からの出力に基づき、GPS車両位置、及び、車両の方位について、観測する。以下に、観測部21が観測する観測量を示す。なお、以下では、観測量として車両の速度、車両の角速度、GPS車両位置、及び、車両の方位を例示する。
【0034】
PLS:車速センサー6からの出力に基づき観測される車両の速度
ωGYR:ジャイロセンサー7からの出力に基づき観測される車両の角速度
GPS:GPS受信部5からの出力に基づき観測されるGPS車両位置のx座標
GPS:GPS受信部5からの出力に基づき観測されるGPS車両位置のy座標
θGPS:GPS受信部5からの出力に基づき観測される車両の方位
【0035】
ここで、上記の観測量をベクトル表記した観測ベクトルを、(vPLS、ωGYR、xGPS、yGPS、θGPS)とすると、観測量についての観測方程式は、式(2)で表される。
【0036】
【数2】
【0037】
式(2)において、rは、観測雑音(平均0、誤差共分散行列であるRを有する正規分布N(0、R))である。
【0038】
以下、カルマンフィルタについて、車両の状態量を予測する予測処理と、車両の状態量を推定する推定処理とに分けて説明する。
【0039】
なお、以下の説明において、添え字のk+1|kが付与された値は、時刻kまでの情報に基づき予測された時刻k+1における予測値を示す。また、添え字のk+1|k+1が付与された値は、時刻k+1までの情報に基づき推定された時刻k+1における推定値を示す。また、添え字のk|kが付与された値は、時刻kまでの情報に基づき推定された時刻kにおける推定値を示す。
【0040】
<予測処理>
カルマンフィルタにおいて予測処理は、車両の状態量の予測値(以下、「車両状態予測値」と表現する)と、車両状態予測値の誤差共分散行列(予測値の誤差)と、を算出する処理である。車両状態予測値は、式(3)に基づき算出される。なお、誤差共分散行列の算出とは、誤差共分散行列の各成分の値の算出を示す。
【0041】
【数3】
【0042】
式(3)では、時刻kまでの情報に基づき予測された時刻k+1における車両状態予測値の算出を示す。この車両状態予測値は、式(3)の右辺に示すように、時刻kまでの情報に基づき推定された時刻kにおける車両の状態量の推定値により算出される。例えば、車両の位置のx座標の予測値を示すxk+1|kは、時刻kまでの情報に基づき推定された時刻kにおける、車両の位置のx座標(xk|k)の推定値と、車両の方位(θk|k)の推定値と、車両の速度(vk|k)の推定値とに基づき算出される。なお、式(3)において、Tは、車速センサー6、ジャイロセンサー7、及び、加速度センサー8からの出力に基づく観測量を、観測部21が観測する間隔を示す。
【0043】
なお、時刻k−1までの情報に基づき予測された時刻kの車両状態予測値を算出する場合、式(3)において時刻を1ステップずつ下げた式により算出できる。つまり、時刻k−1までの情報に基づき予測された時刻kにおける車両状態予測値は、時刻k−1までの情報に基づき推定された時刻k−1における車両の状態量の推定値に基づき算出される。
【0044】
車両状態予測値の誤差共分散行列は、式(4)に基づき算出される。誤差共分散行列は、本実施形態において、車両の状態量に関する分散及び共分散の行列である。分散は、誤差を二乗したものである。つまり、車両の状態量の分散は、車両の状態量の誤差を二乗したものである。したがって、車両状態予測値の誤差共分散行列を算出することは、車両状態予測値の誤差を算出することに相当する。
【0045】
【数4】
【0046】
式(4)において、Pは、誤差共分散行列を示す。式(4)の左辺は、時刻kまでの情報に基づき予測された時刻k+1における誤差共分散行列を示す。式(4)の左辺に示す誤差共分散行列は、時刻kまでの情報に基づき推定された時刻kにおける誤差共分散行列に基づき算出される。なお、Fは、式(1)の状態方程式から求められるヤコビ行列を示す。また、Fにおける添え字のTは、転置行列を示す。また、Qは、システム雑音の誤差共分散行列を示す。
【0047】
なお、時刻k−1までの情報に基づき予測された時刻kにおける車両状態予測値の誤差共分散行列を算出する場合、式(4)においてPの添え字の時刻を1ステップずつ下げた式により算出できる。つまり、時刻k−1までの情報に基づき予測された時刻kにおける車両状態予測値の誤差共分散行列は、時刻k−1までの情報に基づき推定された時刻k−1における車両の状態量の推定値の誤差共分散行列により算出される。
【0048】
このように、予測処理では、例えば、時刻k+1における車両の状態量を予測する場合、時刻kまでの情報に基づき算出された時刻k+1における車両状態予測値と、時刻kまでの情報に基づき算出された時刻k+1における車両状態予測値の誤差共分散行列とを算出する。つまり、予測処理は、車両の状態量の確立分布を予測している。
【0049】
<推定処理>
次に、推定処理について説明する。
カルマンフィルタにおいて推定処理は、予測処理にて算出した車両状態予測値、及び、車両状態予測値の誤差共分散行列に基づいて、車両の状態量の推定値(以下、「車両状態推定値」と表現する)と、車両状態推定値の誤差共分散行列(推定値の誤差)とを算出する処理である。
【0050】
推定処理では、式(5)により観測残差が算出される。観測残差とは、観測量と、車両状態予測値から算出される観測量に対応する値との差である。
【0051】
【数5】
【0052】
式(5)において、左辺は、観測残差をベクトル表記した観測残差ベクトルである。また、式(5)において、右辺の第1項は、時刻kにおいて観測部21が観測した観測量の観測ベクトルである。また、式(5)において、右辺の第2項は、予測処理で予測した車両状態予測値に、観測方程式から求められる観測行列を示す「H」を掛けたものである。
【0053】
車両状態推定値は、式(5)に示す観測残差を用いて、式(6)により算出される。
【0054】
【数6】
【0055】
式(6)では、時刻k+1までの情報に基づき予測された時刻k+1における車両状態推定値を示す。この車両状態推定値は、式(6)の右辺に示すように、時刻kまでの情報に基づき推定された時刻k+1における車両状態予測値を、観測残差より補正して算出される。なお、時刻kまでの情報に基づき予測された時刻kにおける車両状態推定値を算出する場合、時刻k−1までの情報に基づき予測された時刻kにおける車両状態予測値を観測残差により補正することで算出される。
【0056】
前述の通り、式(6)は、観測残差を用いて、車両状態予測値を補正し、車両状態推定値を算出する式を示す。式(6)に示すように、車両状態予測値を観測残差にて補正する際の補正係数としてKが用いられる。Kは、カルマンゲインと呼ばれ、式(7)で表される。
【0057】
【数7】
【0058】
式(7)において、Rは、観測雑音の誤差共分散行列である。本実施形態では、観測雑音の誤差共分散行列とは、観測量の誤差を示す誤差共分散行列である。以下、当該誤差共分散行列を、観測量の誤差共分散行列と表現する。前述した通り、誤差を二乗した分散と、共分散との行列である。したがって、Rは、観測量の誤差を二乗した分散と、共分散との行列である。つまり、観測量の誤差共分散行列は、観測量の誤差に相当する。なお、式(7)において、添え字の「−1」は、逆行列を示す。
【0059】
式(7)に示すカルマンゲインKは、時刻kまでの情報に基づく時刻k+1における車両状態予測値の誤差共分散行列(Pk+1|k)と、時刻kにおいて観測された観測量の誤差共分散行列(R)とに基づき算出される。
【0060】
このカルマンゲインKは、式(6)において、車両状態推定値を算出する際、車両状態予測値を重視して算出するか、観測部21が観測する観測量を重視して算出するかを決定する係数である。
【0061】
例えば、車速センサー6、ジャイロセンサー7、及び、GPS受信部5からの出力に基づく観測量の誤差が、車両状態予測値の誤差より充分に小さい場合、車両状態推定値は、観測量の誤差が充分に小さいことから、観測量となることが望ましい。つまり、観測量の誤差共分散行列であるRの値が、車両状態予測値の誤差共分散行列であるPk+1|kの値より充分に小さい場合、車両状態予測値である式(6)の左辺は、観測量である式(5)の右辺第1項となることが望ましい。これは、車両状態予測値が、誤差が充分に小さい値、換言すると、精度の高い値となるためである。ここで、Rの値が充分に小さい場合のカルマンゲインKをK=H−1として式(6)に与えると、式(6)の右辺の第1項は、式(5)との関係に基づき、無くなる。つまり、車両状態推定値は、誤差が充分に小さい観測量となる。
【0062】
また、例えば、車両状態予測値の誤差が、車速センサー6、ジャイロセンサー7、及び、GPS受信部5からの出力に基づく観測量の誤差より充分に小さい場合、車両状態推定値は、車両状態予測値となることが望ましい。つまり、車両状態予測値の誤差共分散行列であるPk+1|kの値が、観測量の誤差共分散行列であるRの値より充分に小さい場合、車両状態予測値である式(6)の左辺は、車両状態予測値である式(6)の右辺第1項となることが望ましい。これは、車両状態推定値が、観測量の誤差より充分に誤差が小さい値、換言すると、精度の高い値になるためである。ここで、車両状態予測値の誤差共分散行列がRの値より充分に小さい場合のカルマンゲインKとしてK=0として式(6)に与えると、車両状態推定値は、車両状態予測値となる。
【0063】
このように、カルマンゲインKは、観測量の誤差共分散行列であるR、及び、車両状態予測値の誤差共分散行列に応じて、車両状態推定値が適切な値となるように設定する係数である。すなわち、カルマンゲインKは、観測部21が観測する観測量の誤差共分散行列と、車両状態予測値の誤差共分散行列とに基づいて、車両状態推定値が、観測量と車両状態予測値とのうち、誤差共分散行列が小さい、すなわち、誤差が小さいほうを重視した値となるよう設定する係数である。また、カルマンゲインKは、車両状態予測値の誤差共分散行列が正確に予測されていれば、換言すると、当該誤差共分散行列が正確に算出されている場合、車速センサー6、ジャイロセンサー7、及び、GPS受信部5からの出力に基づく観測量の誤差共分散行列に応じて、車両状態推定値が適切な値となるように設定する。
【0064】
車両状態推定値の誤差共分散行列は、式(8)により算出される。前述した通り、誤差共分散行列は、本実施形態では、車両の状態量に関する、分散及び共分散の行列である。車両の状態量の分散は、車両の状態量の誤差を二乗したものである。したがって、車両状態推定値の誤差共分散行列を算出することは、車両状態推定値の誤差を算出することに相当する。
【0065】
【数8】
【0066】
式(8)において、Pは、式(4)と同様、誤差共分散行列を示す。また、式(8)において、Iは、単位行列を示す。式(8)の左辺は、時刻k+1までの情報に基づき推定された時刻k+1における誤差共分散行列を示す。式(8)の左辺に示す誤差共分散行列は、時刻kまでの情報に基づき予測された時刻k+1における誤差共分散行列に基づき算出される。
【0067】
なお、時刻kまでの情報に基づき予測された時刻kにおける車両状態推定値の誤差共分散行列を算出する場合、式(8)においてPの添え字の時刻を1ステップずつ下げた式により算出できる。つまり、時刻kまでの情報に基づき予測された時刻kにおける車両状態推定値の誤差共分散行列は、時刻k−1までの情報に基づき推定された時刻kにおける車両状態予測値の誤差共分散行列により算出される。
【0068】
式(8)は、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列に、(I−KH)を掛けた式である。式(8)に示すように、車両状態推定値の誤差共分散行列は、カルマンゲインKの値に依存する。
【0069】
例えば、観測量の誤差、すなわち、Rが充分に小さい場合のカルマンゲインKとしてK=H−1を式(8)に与えると、車両状態推定値の誤差共分散行列は、零行列となる。これは、推定した車両状態推定値の誤差が充分に小さいことを示す。
【0070】
また、例えば、観測量の誤差より車両状態予測値の誤差が充分に小さい場合のカルマンゲインの値としてK=0を与えることにより、車両状態推定値の誤差共分散行列は、車両状態予測値の誤差共分散行列になる。これは、車両状態推定値の誤差が、観測量の誤差より充分に小さい車両状態予測値の誤差であることを示す。
【0071】
このように、式(8)において、カルマンゲインKは、観測量の誤差共分散行列と、車両状態予測値の誤差共分散行列に応じて、車両状態推定値の誤差共分散行列が適切になるように設定する。つまり、カルマンゲインKは、観測量の精度と車両状態予測値の精度とに基づいて、車両状態推定値の誤差共分散行列が適切になるように設定する。
【0072】
以上のように、推定処理では、例えば、時刻k+1における車両の状態量を推定する場合、時刻k+1までの情報に基づき算出された時刻k+1における車両状態推定値と、時刻k+1までの情報に基づき算出された時刻k+1における車両状態推定値の誤差共分散行列とを算出する。つまり、推定処理は、予測処理で予測した車両の状態量の確立分布に基づいて、車両の状態量の確立分布を推定している。
【0073】
以上の算出により、推定部22は、カルマンフィルタによって、車両状態推定値と、車両状態推定値の誤差共分散行列とを算出することにより、車両の状態を推定する。
【0074】
前述した通り、カルマンゲインKは、車両状態推定値と、車両状態推定値の誤差共分散行列とを適切に設定する係数である。つまり、車両の状態の推定の精度は、カルマンゲインKに依存する。カルマンゲインKは、前述した通り、観測量の誤差共分散行列と、車両状態予測値の誤差共分散行列とに応じて、車両状態推定値と車両状態推定値の誤差共分散行列とを適切に設定する係数である。
【0075】
ところで、カルマンフィルタでは、システム雑音、及び、観測雑音が白色性の雑音であることが前提とされる。つまり、カルマンフィルタにおいて誤差を雑音として扱う場合、当該誤差は、誤差の平均がゼロである白色性の誤差であることが前提となる。したがって、カルマンゲインKの算出において用いられる観測量の誤差は、白色性の誤差であることが前提とされる。しかしながら、観測部21が観測する観測量のうち、少なくともGPS車両位置の誤差は、マルチパスや、GPS衛星の衛星配置等の影響により、誤差の平均がゼロとならない有色性の誤差を示す場合がある。この場合に、カルマンゲインKは、観測量の誤差が有色性の誤差であるため、正確に算出されない。これは、車両状態推定値と車両状態推定値の誤差共分散行列とが精度よく算出できない、すなわち、車両の状態を精度よく推定できないことを示す。
【0076】
そこで、本実施形態の推定部22は、以下に示すように、GPS車両位置の誤差を設定し、カルマンフィルタに基づいて車両の状態を推定する。
【0077】
以下、車両の状態を推定する際のナビゲーション装置1の動作を通して、推定部22による車両の状態の推定について説明する。
【0078】
図2は、ナビゲーション装置1の動作を示すフローチャートである。
【0079】
ナビゲーション装置1の観測部21は、車速センサー6、ジャイロセンサー7、及び、加速度センサー8から出力される信号に基づき、車両の速度、車両の角速度、及び、車両の加速度を観測する(ステップSA1)。観測部21は、車速センサー6、ジャイロセンサー7、及び、加速度センサー8が信号を出力する度に、車両の速度、車両の角速度、及び、車両の加速度を観測する。つまり、観測部21がこれらを観測する間隔は、車速センサー6、ジャイロセンサー7、及び、加速度センサー8が検出する間隔と同じ間隔である。
【0080】
次いで、観測部21は、GPS受信部5の出力に基づき、GPS車両位置、及び、車両の方位を観測する(ステップSA2)。観測部21は、GPS車両位置を示す情報と車両の方位を示す情報とがGPS受信部5から出力される度に、GPS車両位置、及び、車両の方位を観測する。つまり、観測部21がGPS車両位置及び車両の方位を観測する間隔は、GPS受信部5がGPS信号を受信する間隔となる。
【0081】
次いで、ナビゲーション装置1の推定部22は、観測部21が観測した観測量に基づき、車両の状態を推定する車両状態推定処理を実行する(ステップSA3)。
【0082】
図3は、車両状態推定処理における推定部22の動作を示すフローチャートである。
【0083】
推定部22は、予測処理を実行する(ステップSB1)。前述した通り、予測処理は、車両状態予測値、及び、車両状態予測値の誤差共分散行列を算出する処理である。車両状態予測値は、式(3)によって算出される。車両状態予測値の誤差共分散行列は、式(4)によって算出される。
【0084】
次いで、推定部22は、推定処理を実行する(ステップSB2)。推定部22は、推定処理において、式(7)に基づいて、カルマンゲインKを算出し、算出したカルマンゲインK、式(5)、式(6)、及び、式(8)に基づいて、車両状態推定値と車両状態推定値の誤差共分散行列とを算出する。
【0085】
推定部22は、式(7)に基づいて、カルマンゲインKを算出する際、観測量の誤差共分散行列が含む成分のうち、GPS車両位置の誤差を、式(9)に示すように、設定する。なお、以下の説明において、便宜上、観測量の誤差共分散行列であるRに入力するGPS車両位置の誤差を、GPS車両位置の観測誤差と表現し、観測部21がGPS受信部5からの出力に基づき観測するGPS車両位置の誤差を、GPS車両位置の測位誤差と表現する。
【0086】
【数9】
【0087】
式(9)は、GPS車両位置の観測誤差が、GPS車両位置の測位誤差に√(2τ)を掛けたもので表されることを示す。これは、推定部22が、GPS車両位置の測位誤差に、τに基づく√(2τ)の重み付けを付与し、重み付けを付与したGPS車両位置の測位誤差をGPS車両位置の観測誤差として設定することを示す。
【0088】
式(9)において、τは、相関時定数を示す。相関時定数とは、あるタイミング(第1のタイミング)から、当該あるタイミングにおいて受信したGPS信号が、観測部21が観測するGPS車両位置の観測に反映しない当該あるタイミングと異なる他のタイミング(第2のタイミング)までの期間を示す。ここで、相関時定数について、詳述する。
【0089】
図4は、相関時定数を説明するための図である。
図4では、車両が、方向YZに延びる直線TS上を、方向YZ1に向かって走行する場合を例示する。
【0090】
タイミングT1(第1のタイミング)において、GPS信号の受信に関する影響(以下、「GPS受信影響」と表現する)(第1の影響)が発生したとする。GPS受信影響は、例えば、マルチパスの影響や、GPS衛星の衛星配置に起因したGPS信号の受信強度の低下の影響等が挙げられる。
【0091】
図4では、タイミングT1における実際の車両の位置を、位置MT1とする。前述した通り、図4では、車両が直線TS上を走行する場合を例示するため、位置MT1は、直線TS上に位置する。
【0092】
また、図4では、タイミングT1において観測部21が観測するGPS車両位置を、位置NT1とする。図4に示すように、位置NT1は、直線TSと同様に方向YZに延びる仮想直線KT上に位置する。仮想直線KTは、タイミングT1で発生したGPS受信影響によって実際の車両の位置とGPS車両位置とに誤差(以下、「離間誤差」と表現する)が発生した場合において、観測部21が観測するGPS車両位置が位置する仮想の直線である。本実施形態では、仮想直線KTは、直線TSと並行な直線である。
【0093】
このように、図4では、タイミングT1におけるGPS受信影響の発生により、観測部21が、実際の車両の位置である位置MT1から距離l1離れた位置NT1をGPS車両位置として観測する場合を例示する。つまり、図4では、タイミングT1において、GPS受信影響により、観測部21が観測するGPS車両位置と、実際の車両の位置との間に、距離l1の離間誤差が発生したことを示している。
【0094】
GPS受信影響の発生によるGPS車両位置の測位誤差は、GPS受信影響がない場合のGPS車両位置の測位誤差と比較して大きい誤差である。図4の場合、タイミングT1において観測部21が観測するGPS車両位置の測位誤差、すなわち、位置NT1の誤差は、タイミングT1で発生したGPS受信影響により、当該GPS受信影響がない場合のGPS車両位置の測位誤差と比較して大きい誤差である。
【0095】
ここで、GPS受信影響によりGPS車両位置の測位誤差が大きい誤差となることについて、以下に一例を挙げて説明する。
GPS受信部5は、受信したGPS信号が含むGPS衛星の位置やGPS信号を送信した時刻等の情報や、GPS信号を受信した時刻等に基づいて、GPS衛星と車両との距離(以下、「擬似距離」と表現する)を測定する。GPS受信部5は、複数のGPS衛星について得られた擬似距離に基づいて所定の演算することにより、GPS車両位置を算出する。ここで、GPS受信影響としてマルチパスの影響が発生したとする。マルチパスとは、GPS衛星から直接到来する直接波と、建物等での反射による反射波との複数の経路からGPS信号を受信する現象である。このように複数の経路からGPS信号を受信するため、GPS衛星から車両への到達時間は、直接波のGPS信号と反射波のGPS信号とにおいて異なる。そのため、反射波のGPS信号や、直接波と反射波が合成された合成波のGPS信号等に基づき算出される擬似距離は、直接波のGPS信号に基づき算出される擬似距離と比較して、擬似距離に大きな誤差が発生する。したがって、擬似距離に対する大きな誤差の発生によって、観測部21が観測するGPS車両位置の測位誤差は、マルチパスの影響のないGPS車両位置の測位誤差と比較して大きくなる。
【0096】
図4の説明に戻り、タイミングT2において、タイミングT1で発生したGPS受信影響が継続しているとする。図4では、タイミングT2における実際の車両の位置を、位置MT2とする。位置MT1と同様、位置MT2は、直線TS上に位置する。また、図4では、タイミングT2において観測部21が観測するGPS車両位置を、位置NT2とする。
【0097】
図4では、タイミングT2において、タイミングT1で発生したGPS受信影響により、観測部21が、実際の車両の位置である位置MT2から距離l2離れた位置NT2を、GPS車両位置として観測する場合を例示する。つまり、図4では、タイミングT2において、タイミングT1で発生したGPS受信影響により、観測部21が観測するGPS車両位置と、実際の車両の位置との間に、距離l2の離間誤差が発生したことを示している。
【0098】
前述した通り、GPS受信部5は、前回算出したGPS車両位置を加味して、受信したGPS信号に基づくGPS車両位置を算出する。したがって、図4において、GPS受信部5は、タイミングT2におけるGPS車両位置である位置NT2を算出する際、前回算出した位置、すなわち、タイミングT1において算出したGPS車両位置である位置NT1を加味して算出する。図4では、位置NT2が仮想直線KT上に位置することを示している。このように、前回算出した位置を加味することにより、GPS受信部5は、実際の車両の位置の変化に対して、GPS車両位置の変化が乖離することを抑制する。つまり、図4の場合、車両が直線TS上を方向YZ1に向かって走行している場合、前回算出した位置を加味することにより、GPS車両位置が、仮想直線KT上を方向YZ1に向かって移動するように、GPS車両位置を算出する。これによって、GPS受信部5は、実際の車両の位置が直線TS上を方向YZ1に向かって変化することに合わせて、GPS車両位置が仮想直線KT上を方向YZ1に向かって変化するよう、GPS車両位置を算出する。図4では、位置NT1と位置NT2とが仮想直線KT上に位置する場合を例示するため、離間誤差である距離l1と距離l2とは、同じ値である。
【0099】
このように、前回算出した位置を加味するため、GPS受信部5は、タイミングT1で受信したGPS信号に基づき算出されたGPS車両位置、すなわち、位置NT1が反映されたGPS車両位置を算出する。つまり、観測部21は、タイミングT2において、タイミングT1で受信したGPS信号が反映されたGPS車両位置、すなわち、位置NT2を観測する。
【0100】
図4のタイミングT2において観測部21が観測するGPS車両位置の測位誤差、すなわち、位置NT2の誤差は、タイミングT1で発生したGPS受信影響により、当該GPS受信影響がない場合のGPS車両位置の測位誤差と比較して大きい誤差である。
【0101】
ここで、位置NT1の誤差と、位置NT2の誤差とは、相関関係にある。これは、位置NT1の誤差及び位置NT2の誤差が、GPS受信影響がない場合の誤差と比較して大きい誤差であることから、タイミングT1で発生したGPS受信影響に基づく誤差を含むためである。具体的には、位置NT1の誤差及び位置NT2の誤差は、同じGPS受信影響に基づく誤差を含むことから、互いに当該GPS受信影響に応じた誤差となる。そのため、位置NT1の誤差及び位置NT2の誤差は、相関関係にある。このように、位置NT1の誤差及び位置NT2の誤差は、相関関係にあることから、相関性を有する誤差である。相関性を有する誤差は、誤差の平均がゼロにならない誤差である。したがって、位置NT1の誤差及び位置NT2の誤差は、誤差の平均がゼロにならない誤差、すなわち、有色性の誤差である。
【0102】
タイミングT3においても、タイミングT1において発生したGPS受信影響が継続しているとする。図4では、タイミングT3における実際の車両の位置を、位置MT3とする。位置MT1、及び、位置MT2と同様、位置MT3は、直線TS上に位置する。また、図4では、タイミングT3において観測部21が観測するGPS車両位置を、位置NT3とする。
【0103】
図4では、タイミングT3において、タイミングT1で発生したGPS受信影響により、観測部21が、実際の車両の位置である位置MT3から距離l3離れた位置NT2を、GPS車両位置として観測する場合を例示する。つまり、図4では、タイミングT3において、タイミングT1で発生したGPS受信影響により、観測部21が観測するGPS車両位置と、実際の車両の位置との間に、距離l3の離間誤差が発生したことを示している。
【0104】
GPS受信部5は、タイミングT3におけるGPS車両位置である位置NT3を算出する際、タイミングT2において算出したGPS車両位置を加味して算出する。タイミングT2において算出したGPS車両位置は、位置NT1を加味して算出したものである。したがって、GPS受信部5は、タイミングT3において、タイミングT1で受信したGPS信号に基づき算出されたGPS車両位置が反映されたGPS車両位置を算出する。つまり、観測部21は、タイミングT3において、タイミングT1で受信したGPS信号が反映されたGPS車両位置を観測する。
【0105】
また、GPS受信部5は、前回算出したGPS車両位置を加味するため、実際の車両の位置の変化に対して、GPS車両位置の変化が乖離することを抑制した位置NT3を算出する。つまり、GPS受信部5は、タイミングT1と同じGPS受信影響がタイミングT3においてあるため、仮想直線KT上に位置するように位置NT3を算出する。図4では、位置NT3が仮想直線KT上に位置する場合を例示するため、離間誤差である距離l3は、距離l1と距離l2と同じ値である。
【0106】
図4のタイミングT3において観測部21が観測するGPS車両位置の測位誤差、すなわち、位置NT3の誤差は、タイミングT1で発生したGPS受信影響により、GPS受信影響がない場合の誤差と比較して大きい誤差である。
【0107】
ここで、位置NT2の誤差と、位置NT3の誤差とは、相関関係にある。これは、位置NT2の誤差及び位置NT3の誤差が、GPS受信影響がない場合の誤差と比較して大きい誤差であることから、タイミングT1で発生したGPS受信影響に基づく誤差を含むためである。具体的には、位置NT2の誤差及び位置NT3の誤差は、タイミングT1で発生したGPS受信影響に基づく誤差を含むことから、互いに当該GPS受信影響に応じた誤差となり、相関関係にある。このように、位置NT1の誤差及び位置NT2の誤差は、相関関係にあることから、相関性を有する誤差である。前述した通り、相関性を有する誤差は、誤差の平均がゼロにならない誤差である。したがって、位置NT1の誤差及び位置NT2の誤差は、誤差の平均がゼロにならない誤差、すなわち、有色性の誤差である。
【0108】
タイミングT4においても、タイミングT1において発生したGPS受信影響が継続しているとする。図4では、タイミングT4における実際の車両の位置を、位置MT4とする。また、図4では、タイミングT4において、観測部21が観測するGPS車両位置を、位置NT4とする。
【0109】
図4では、タイミングT4において、タイミングT1で発生したGPS受信影響により、観測部21が、実際の車両の位置である位置MT4から距離l4離れた位置NT4を、GPS車両位置として観測する場合を例示する。つまり、図4では、タイミングT4において、タイミングT1において発生したGPS受信影響により、観測部21が観測するGPS車両位置に対して、実際の車両の位置から距離l4の離間誤差が発生したことを示している。
【0110】
GPS受信部5は、タイミングT4におけるGPS車両位置である位置NT4を算出する際、タイミングT3において算出したGPS車両位置を加味して算出する。タイミングT3において算出したGPS車両位置は、位置NT2を加味して算出したものである。したがって、GPS受信部5は、タイミングT4において、タイミングT1で受信したGPS信号に基づき算出されたGPS車両位置が反映されたGPS車両位置を算出する。つまり、観測部21は、タイミングT4において、タイミングT1で受信したGPS信号が反映されたGPS車両位置を観測する。
【0111】
また、GPS受信部5は、前回算出したGPS車両位置を加味するため、実際の車両の位置の変化に対して、GPS車両位置の変化が乖離することを抑制した位置NT4を算出する。つまり、GPS受信部5は、タイミングT1と同じGPS受信影響がタイミングT4においてあるため、仮想直線KT上に位置するように位置NT4を算出する。図4では、位置NT4が仮想直線KT上に位置する場合を例示するため、離間誤差である距離l3は、距離l1と距離l2と距離l3と同じ値である。
【0112】
図4のタイミングT4において観測部21が観測するGPS車両位置の誤差、すなわち、位置NT4の誤差は、タイミングT1で発生したGPS受信影響により、GPS受信影響がない場合のGPS車両位置の測位誤差と比較して大きい誤差である。
【0113】
ここで、位置NT3の誤差と、位置NT4の誤差とは、相関関係にある。これは、位置NT3の誤差及び位置NT4の誤差が、GPS受信影響がない場合の誤差と比較して大きい誤差であることから、タイミングT1で発生したGPS受信影響に基づく誤差を含むためである。具体的には、位置NT3の誤差及び位置NT4の誤差は、タイミングT1で発生したGPS受信影響に基づく誤差を含むことから、互いに当該GPS受信影響に応じた誤差となり、相関関係にある。このように、位置NT3の誤差及び位置NT4の誤差は、相関関係にあることから、相関性を有する誤差である。したがって、位置NT3の誤差及び位置NT4の誤差は、誤差の平均がゼロにならない誤差、すなわち、有色性の誤差である。
【0114】
ここまで説明したように、図4では、タイミングT1からタイミングT4のそれぞれのGPS車両位置の測位誤差が、タイミングT1で発生したGPS受信影響に基づく誤差を含む場合を示す。したがって、図4において、位置NT1から位置NT4のそれぞれの誤差は、互いに相関関係にある。
【0115】
タイミングT5(第2のタイミング)において、タイミングT1で発生したGPS受信影響と異なるGPS受信影響(第2の影響)が発生したとする。
【0116】
図4では、タイミングT5において、実際の車両の位置を、位置MT5とする。また、図4では、タイミングT5において、観測部21が観測するGPS車両位置を、位置NT5とする。つまり、タイミングT5において発生したGPS信号に基づく影響により、観測部21は、実際の車両の位置である位置MT5から距離l5離れた位置NT5をGPS車両位置として観測する。これは、タイミングT5において発生したGPS受信影響により、観測部21が観測するGPS車両位置に対して、実際の車両の位置から距離l5の離間誤差があることを示す。
【0117】
図4に示すようにタイミングT5における離間誤差は、タイミングT1からタイミングT4のそれぞれにおける離間誤差と比較して異なる。図4では、離間誤差を示す距離l5は、距離l1から距離l4のいずれの離間誤差よりも短い場合を例示する。つまり、図4では、タイミングT5で発生したGPS受信影響に基づく離間誤差が、タイミングT1で発生したGPS受信影響に基づく離間誤差と比較して小さい場合を例示する。このようにタイミングT5における離間誤差が他の離間誤差と異なるのは、タイミングT1で発生したGPS受信影響と異なるGPS受信影響が、タイミングT5において発生したことに起因する。
【0118】
前述した通り、GPS受信部5は、前回算出したGPS車両位置を加味して、受信したGPS信号に基づくGPS車両位置を算出する。そのため、GPS受信部5は、タイミングT5におけるGPS車両位置である位置NT5を算出する際、タイミングT4において算出したGPS車両位置を加味して算出する。タイミングT4において算出したGPS車両位置は、位置NT3を加味して算出したものであり、タイミングT1において受信したGPS信号が反映されて算出されている。しかしながら、タイミングT5において算出したGPS車両位置は、タイミングT1において受信したGPS信号が反映されないGPS車両位置と見做すことができる。これは、図4に示すように、タイミングT5において観測したGPS車両位置が、仮想直線KT上に位置していないためである。このように、タイミングT5において仮想直線KT上に位置していないのは、タイミングT5において、タイミングT1で発生したGPS受信影響と異なるGPS受信影響が発生し、タイミングT1からタイミングT4で発生した離間誤差が異なることに起因する。したがって、タイミングT5において算出したGPS車両位置は、タイミングT1で受信したGPS信号に基づき算出されたGPS車両位置が反映されないGPS車両位置と見做すことができる。つまり、これは、観測部21が、タイミングT5において、タイミングT1で受信したGPS信号が反映されないGPS車両位置を観測することに相当する。
【0119】
図4のタイミングT5において観測部21が観測するGPS車両位置の測位誤差、すなわち、位置NT5の誤差は、タイミングT5で発生したGPS受信影響により、当該GPS受信影響がない場合のGPS車両位置の測位誤差と比較して大きい誤差である。また、タイミングT5において観測部21が観測するGPS車両位置の測位誤差は、GPS受信影響が異なるため、タイミングT1からタイミングT4のそれぞれのタイミングにおけるGPS車両位置の測位誤差と異なる誤差である。したがって、位置NT5の誤差は、位置NT1から位置NT4のそれぞれの誤差と相関関係にない。
【0120】
図4において、相関時定数とは、以下の期間を示す。すなわち、図4において相関時定数は、タイミングT1(第1のタイミング)から、タイミングT1で受信したGPS信号が観測部21によるGPS車両位置の観測に反映しないタイミングT5(第2のタイミング)までの期間である。より具体的には、図4において相関時定数とは、タイミングT1においてGPS受信影響(第1の影響)が発生し、観測されるGPS車両位置に対して当該GPS受信影響に基づく離間誤差が発生してから、当該GPS受信影響と異なるGPS受信影響(第2の影響)が発生し、この異なるGPS受信影響に基づく離間誤差がGPS車両位置に対して発生するまでの期間である。
【0121】
なお、図4では、相関時定数について、上述した期間に限定されない。例えば、GPS受信影響が発生し、当該GPS受信影響に基づく実際の車両の位置とGPS車両位置との離間誤差が発生してから、当該離間誤差が解消するまでの期間でもよい。つまり、図4の場合、この期間は、タイミングT1から、タイミングT1で発生したGPS受信影響により離間誤差が解消するまでの期間である。ここで解消とは、離間距離が、タイミングT5におけるGPS受信影響による離間距離となるのではなく、ゼロに近い値となること、すなわち、実際の車両の位置とGPS車両位置との間の誤差がなくなることを示す。
【0122】
この相関時定数は、事前のテストやシミュレーション等に基づいて算出され、記憶部等に記憶される。本実施形態では、山道や、高速道路の下に設けられる車道等の種々の車道について算出された、複数の上記に示す期間のうち、最も長い期間が、相関時定数として記憶部等に記憶される。
【0123】
前述した通り、カルマンゲインKを算出する際、観測量の誤差共分散行列が含む成分のうち、GPS車両位置の観測誤差を、式(9)に示すように設定する。つまり、推定部22は、GPS車両位置の測位誤差に√(2τ)を掛けたものをGPS車両位置の観測誤差として設定する。
【0124】
式(9)は、以下により算出される。
【0125】
GPS車両位置の誤差が、相関時定数であるτ、及び、GPS車両位置の測位誤差であるσの一次マルコフ過程に従うとすると、GPS車両位置の誤差の自己相関関数は、式(10)で表される。自己相関関数とは、本実施形態の場合、時刻に対する、GPS車両位置の誤差の自己の相関を示す関数である。
【0126】
【数10】
【0127】
式(10)は、GPS車両位置の誤差の自己相関関数が、相関時定数であるτと、GPS車両位置の測位誤差であるσとに基づくエクスポネンシャル関数であることを示す。式(10)に示すように、時刻tが大きければ大きいほど、GPS車両位置の誤差の自己の相関は、大きくなる。
【0128】
GPS車両位置の誤差の自己相関関数が式(10)で表される場合、ウィーナー=ヒンチンの定理、及び、パワースペクトルと分散との関係に基づき、下記に示す式(11)が求められる。
【0129】
【数11】
【0130】
式(11)において、Err(t)は、GPS車両位置の誤差を示す。また、式(11)において、σは、時刻tにおけるGPS車両位置の測位誤差を示す。式(11)は、式(11)の左辺を右辺に移項させ、式(11)の右辺の第1項を左辺に移項させ、τで整理することにより、式(12)に変形できる。
【0131】
【数12】
【0132】
式(12)において、右辺の第1項のErr(t)/dtは、GPS車両位置の誤差の変化量を示す。つまり、式(12)は、GPS車両位置の誤差が、GPS車両位置の誤差の変化量に「−τ」を掛けたものと、時刻tにおけるGPS車両位置の測位誤差に「√(2τ)」を掛けたものの和によって表されることを示す。
【0133】
ここで、式(12)の右辺の第1項のErr(t)/dtは、以下に示す理由により、ゼロと見做すことができる。
【0134】
GPS車両位置の誤差の変化量は、観測や推定等が困難な量である。そのため、GPS車両位置の誤差の変化量の期待値は、ゼロであることが考えられる。また、GPS受信部5の演算により、GPS車両位置は、誤差に変動のない位置、すなわち、誤差が誤差に対する変化量を有さない位置として算出される。このことから、GPS車両位置の誤差の変化量は、ゼロであることが考えられる。以上から、GPS車両位置の誤差の変化量は、ゼロと見做すことができる。
【0135】
以上の理由からGPS車両位置の誤差の変化量をゼロとすると、式(12)は、右辺の第1項が無くなり、式(13)で表される。
【0136】
【数13】
【0137】
式(13)は、時刻tにおけるGPS車両位置の誤差が、時刻tにおけるGPS車両位置の測位誤差に√(2τ)を掛けることによって表記されることを示す。ここで、式(13)の右辺は、式(9)の右辺に相当し、GPS車両位置の観測誤差である。すなわち、GPS車両位置の観測誤差は、GPS車両の測位誤差に、相関時定数であるτに基づく√(2τ)の重み付けを付与することによって表される。
【0138】
推定部22は、カルマンゲインKを算出する際、観測量の誤差共分散行列に含まれる成分のうちGPS車両位置について、式(13)に基づいてGPS車両位置の観測誤差を設定する。つまり、推定部22は、観測部21が観測したGPS車両位置の測位誤差に、√(2τ)の重み付けを付与し、重み付けを付与したGPS車両位置の測位誤差をGPS車両位置の観測誤差として、カルマンゲインKの観測量の誤差共分散行列に入力する。より具体的には、推定部22は、観測量の誤差共分散行列に含まれる成分のうちGPS車両位置について、式(14)及び式(15)に示すようにGPS車両位置の観測誤差を設定し、カルマンゲインKの観測量の誤差共分散行列に入力する。
【0139】
【数14】
【0140】
式(14)において、添え字の「k」は、時刻を示す。また、添え字の「xGPS」及び「gx」は、x軸成分を示す。つまり、式(14)の右辺は、時刻kのx軸におけるGPS車両位置の観測誤差を示す。また、式(14)の左辺は、√(2τ)に、時刻kのx軸におけるGPS車両位置の測位誤差を掛けたもの示す。つまり、GPS車両位置の観測誤差のx軸成分は、GPS車両位置の測位誤差のx軸成分に√(2τ)の重み付けが付与され、設定される。
【0141】
【数15】
【0142】
式(15)において、添え字の「k」は、時刻を示す。また、添え字の「yGPS」及び「gy」は、y軸成分を示す。つまり、式(15)の右辺は、時刻kのy軸におけるGPS車両位置の観測誤差を示す。また、式(15)の左辺は、√(2τ)に、時刻kのy軸におけるGPS車両位置の測位誤差を掛けたもの示す。つまり、GPS車両位置の観測誤差のy軸成分は、GPS車両位置の測位誤差のy軸成分に√(2τ)の重み付けが付与され、設定される。
【0143】
√(2τ)の重み付けの付与は、τが示す期間において、有色性の誤差であるGPS車両位置の測位誤差を、まとめて、1のGPS車両位置の測位誤差(以下、「集GPS車両位置測位誤差」と表現する)としてカルマンフィルタで扱うための付与である。前述した通り、τは、相関時定数であり、あるタイミングで受信したGPS信号が、観測部21によるGPS車両位置の観測に反映しない当該あるタイミングと異なる他のタイミングまでの期間である。そして、τは、あるタイミングで発生したGPS受信影響に基づく有色性の誤差であるGPS車両位置の測位誤差を含む期間である。推定部22は、この√(2τ)の重み付けを付与することにより、τの期間において有色性の誤差であるGPS車両位置の測位誤差をまとめて、集GPS車両位置測位誤差としてカルマンフィルタで扱い、カルマンゲインKを算出する。
【0144】
集GPS車両位置測位誤差は、白色性の誤差と見做すことができる。これは、集GPS車両位置測位誤差と、τの期間において有色性の誤差であるGPS車両位置の測位誤差を除く、他のGPS車両位置の測位誤差と、に相関関係がないためである。例えば、図4の場合、タイミングT1からタイミングT4までのGPS車両位置の測位誤差を、まとめて、集GPS車両位置測位誤差とした場合、この集GPS車両位置測位誤差は、GPS受信影響が異なるため、タイミングT5のGPS車両位置の測位誤差と相関関係にない。また、集GPS車両位置の測位誤差は、タイミングT1の前回のタイミングにおけるGPS車両位置の測位誤差とも相関関係にない。図4では、タイミングT1でGPS受信影響が発生した場合を例示している。そのため、タイミングT1の前回のタイミングでGPS受信影響があったとしても、このGPS受信影響は、タイミングT1で発生したGPS受信影響と異なる。したがって、集GPS車両位置測位誤差は、タイミングT1の前回のタイミングにおけるGPS車両位置の測位誤差とも相関関係にない。このように、τの期間において有色性の誤差であるGPS車両位置の測位誤差を、まとめて、集GPS車両位置測位誤差とすることにより、この集GPS車両位置測位誤差は、白色性の誤差と見做すことができる。
【0145】
また、集GPS車両位置測位誤差は、τの期間において有色性の誤差を示すGPS車両位置の測位誤差を全て含むGPS車両位置の測位誤差である。例えば、図4において、タイミングT1からタイミングT4におけるGPS車両位置の測位誤差、すなわち、位置NT1から位置NT4のそれぞれの誤差を、集GPS車両位置測位誤差とした場合、集GPS車両位置測位誤差は、位置NT1から位置NT4のそれぞれの誤差を含む。したがって、集GPS車両位置測位誤差が白色性の誤差として見做せるため、集GPS車両位置測位誤差に含まれたGPS車両位置の測位誤差のそれぞれは、白色性の誤差の一部に相当する。これは、τの期間において有色性の誤差を示すGPS車両位置の測位誤差をまとめて、集GPS車両位置測位誤差とすることにより、τの期間において有色性の誤差を示すGPS車両位置の測位誤差が、白色性の誤差の一部としてカルマンフィルタで扱われることを示す。すなわち、τの期間において有色性の誤差を示すGPS車両位置の測位誤差をまとめて、集GPS車両位置測位誤差としてカルマンフィルタで扱うことで、τの期間において有色性の誤差を示すGPS車両位置の測位誤差を、白色性の誤差として見做すことができる。
【0146】
例えば、図4において、タイミングT1からタイミングT4におけるGPS車両位置の測位誤差、すなわち、位置NT1から位置NT4のそれぞれの誤差を、集GPS車両位置測位誤差とした場合、√(2τ)を付与した位置NT1から位置NT4のそれぞれの誤差は、白色性の誤差の一部としてカルマンフィルタで扱われる。このように、位置NT1から位置NT4のそれぞれの誤差をまとめて、集GPS車両位置測位誤差としてカルマンフィルタで扱うことで、位置NT1から位置NT4のそれぞれの誤差を白色性の誤差として見做すことができる。
【0147】
前述した通り、√(2τ)の重み付けの付与は、τが示す期間において、有色性の誤差であるGPS車両位置の測位誤差をまとめて、集GPS車両位置測位誤差としてカルマンフィルタで扱うための付与である。したがって、√(2τ)の重み付けが付与されたGPS車両位置の測位誤差、すなわち、式(13)の左辺は、集GPS車両位置測位誤差の一部であり、白色性の誤差であると見做すことができる。つまり、式(13)のように設定したGPS車両位置の観測誤差は、白色性の誤差として見做すことができる。これにより、GPS車両位置の観測誤差を、式(13)のように設定することにより、白色性の誤差が前提となる観測量の誤差共分散行列に対してGPS車両位置の観測誤差を適切に入力でき、推定部22は、カルマンゲインKを正確に算出できる。そして、推定部22は、カルマンゲインKが正確に算出されるため、車両の現在地に関する状態を精度よく推定できる。
【0148】
また、式(13)のようにGPS車両位置の観測誤差を設定することにより、推定部22は、GPS信号の受信するたびに、車両の現在地の状態を精度よく推定できる。前述したように、カルマンフィルタにおいて、観測量の誤差共分散行列が含む誤差は、白色性の誤差が前提とされる。そのため、GPS受信影響が発生した場合、GPS信号を受信するたびに、GPS車両位置の測位誤差をそのままGPS車両位置の観測誤差としてカルマンフィルタに入力すると、車両の現在地の状態の推定の精度が低下する。これに対し、τの期間あけて、GPS車両位置の測位誤差をそのままGPS車両位置の観測誤差としてカルマンフィルタに入力し、車両の現在地の状態の推定することが考えられる。しかしながら、これでは、車両の現在地に関する状態の推定の頻度が低下、また、推定の精度の低下の懸念がある。ここで、式(13)のようにGPS車両位置の観測誤差を設定することで、τの期間のGPS車両位置の測位誤差が有色性の誤差であっても、カルマンフィルタでは白色性の誤差として扱われる。したがって、式(13)のようにGPS車両位置の観測誤差を設定することにより、推定部22は、GPS信号の受信するたびに、車両の現在地の状態を精度よく推定できる。
【0149】
図4では、τが示す期間は、タイミングT1においてGPS受信影響が発生し、観測されるGPS車両位置に対して、当該GPS受信影響に基づく離間誤差が発生してから、当該GPS受信影響と異なるGPS受信影響が発生し、この異なるGPS受信影響に基づく離間誤差がGPS車両位置に対して発生するまでの期間である。推定部22は、この期間に基づく重み付け(√(2τ))の付与により、この期間において有色性の誤差であるGPS車両位置の測位誤差を、白色性の誤差と見做せるGPS車両位置の観測誤差として設定できる。そのため、推定部22は、設定したGPS車両位置の観測誤差を、観測量の誤差共分散行列(R)に入力することで、カルマンゲインKを正確に算出できる。したがって、推定部22は、この期間における車両の現在地に関する状態を精度よく推定できる。
【0150】
また、図4では、τが示す期間は、GPS受信影響が発生し、当該GPS受信影響に基づく離間誤差がGPS車両位置に対して発生してから、当該離間誤差が解消するまでの期間でもよい。このような期間であっても、推定部22は、期間に基づく重み付け(√(2τ))の付与により、このような期間において有色性の誤差であるGPS車両位置の測位誤差を、白色性の誤差と見做せるGPS車両位置の観測誤差として設定できる。そのため、推定部22は、設定したGPS車両位置の観測誤差を、観測量の誤差共分散行列(R)に入力することで、カルマンゲインKを正確に算出できる。したがって、推定部22は、GPS受信影響が発生し、当該GPS受信影響に基づく離間誤差がGPS車両位置に対して発生してから、当該離間誤差が解消するまでの期間における車両の現在地に関する状態を、精度よく推定できる。
【0151】
なお、ナビゲーション装置1がGPS受信影響の有無を判別可能な構成である場合において、GPS受信影響であると判別した場合、GPS車両位置の観測誤差を式(16)及び式(17)のように設定してもよい。
【0152】
【数16】
【0153】
VARrxは、時刻kにおける、GPS車両位置のx軸成分と、予測処理で予想した車両の位置(車両状態予測値における車両の位置)のx軸成分との分散を示す。つまり、式(16)は、式(14)に当該分散を加えてGPS車両位置の観測誤差を設定する。
【0154】
【数17】
【0155】
VARryは、時刻kにおける、GPS車両位置のy軸成分と、予測処理で予想した車両の位置(車両状態予測値の車両の位置)のy軸成分との分散を示す。つまり、式(17)は、式(15)に当該分散を加えてGPS車両位置の観測誤差を設定する。
【0156】
このように、GPS受信影響の有無の判定が可能に構成され、GPS受信影響がある場合、GPS車両位置の観測誤差を式(16)及び式(17)に示すように、設定する。これにより、GPS車両位置と予測処理で予測した車両の位置との分散を加えることで、GPS車両位置の観測誤差がこれら位置に基づく分散を含むため、より正確にGPS車両位置の観測誤差が算出される。そのため、カルマンゲインKがより正確に算出され、推定部22は、車両の現在地に関する状態を精度よく推定できる。
【0157】
なお、式(16)及び式(17)は、GPS受信影響の有無の判定が可能に構成され、GPS受信影響がある場合に、GPS車両位置の観測誤差を設定する式である。しかしながら、式(16)及び式(17)は、GPS受信影響の有無に関係なく用いられてもよい。これは、GPS受信影響がない場合、式(16)のVARrxと式(17)のVARryは、0に近似することが考えられる。したがって、式(16)及び式(17)を、GPS受信影響の有無に関係なく、GPS車両位置の観測誤差の設定に用いてもよい。この場合でも、上述した効果を奏する。
【0158】
前述した通り、ナビゲーション装置1の制御部2は、推定した車両の状態に基づき、マップマッチングの対象となるリンクの評価を実行する。
【0159】
図5は、推定した車両の状態に基づくマップマッチングの処理を説明するため、地図上の道路R1と、道路R2とを示す図である。図5において、道路R1は、方向Y1に向かって延在する道路である。また、道路R2は、方向Y1と並行でない方向Y2に向かって延在する道路である。
【0160】
図5において、リンクL1は、道路R1に対応するリンクである。また、リンクL2は、道路R2に対応するリンクである。
【0161】
また、図5において、マークαは、推定部22により推定された車両の位置を示すマークである。以下、図5を用いた説明では、推定部22により、車両が、位置M1に位置し、進行方向X1に向かって走行しているものと推定されたとする。図5に示すように、位置M1は、道路R1上の位置であって、道路R1の幅方向の中心から、方向Y1に向かって右側に離間した位置である。
【0162】
道路R1と、道路R2と、推定された車両の位置との関係が、図5に示す関係である場合、マップマッチングに際し、まず、制御部2は、地図データ3aを参照し、マップマッチングの候補となるリンクであるマップマッチング候補リンクを取得する。制御部2は、マップマッチング候補リンクとして、推定された車両の位置から予め設定された所定の範囲内に位置し、かつ、車両とリンクとの方位の誤差が所定範囲内である1又は複数のリンクを取得する。図5の例では、リンクL1、及び、リンクL2は、推定された車両の位置である位置M1から所定範囲に位置し、また、車両とリンクとの方位の誤差が所定範囲内のリンクである。このため、制御部2は、リンクL1、及び、リンクL2をマップマッチング候補リンクとして取得する。
【0163】
次いで、制御部2は、取得したマップマッチング候補リンクのそれぞれについて、リンクを評価する評価量を算出する。
【0164】
評価量は、推定された車両の位置とマップマッチング候補リンクの位置との誤差、及び、推定された車両の方位とマップマッチング候補リンクの方位との誤差に基づいて、下記に示す式(18)により算出される値である。
【0165】
τ=δx/Δx+δy/Δy+δθ/Δθ・・・(18)
【0166】
ここで、推定された車両の位置とマップマッチング候補リンクの位置との誤差とは、推定された車両の位置からマップマッチング候補リンクに対して垂線を延ばした場合における垂線及びマップマッチング候補リンクとの交点と、推定された車両の位置との、x軸方向、及び、y軸方向における差である。式(18)において、δxは、当該交点のx座標と、推定された車両の位置のx座標との差を示す。また、δyは、当該交点のy座標と、推定された車両の位置のy座標との差を示す。
【0167】
図5の例において、推定された車両の位置とリンクL1との位置の誤差は、位置M1からリンクL1に対して延ばした垂線S1及びリンクL1の交点MM1と、位置M1とのx軸方向、及び、y軸方向における差である。また、図5において、推定された車両の位置とリンクL2との位置の誤差は、位置M1からリンクL2に対して延ばした垂線S2及びリンクL2の交点MM2と、位置M1とのx軸方向、及び、y軸方向における差である。
【0168】
また、推定された車両の方位とマップマッチング候補リンクの方位との誤差とは、推定された車両の方位に対応する角度と、マップマッチング候補リンクの方位に対応する角度との差である。
【0169】
前述した通り、車両の方位とは、車両の進行方向の方角を意味する。図5の例では、推定された車両の方位は、進行方向X1の方角である。また、車両の方位に対応する角度とは、東に向かう方向を基準として、東に向かう方向と、車両の方位との反時計回りの離間角度を意味する。図5の例では、推定された車両の方位に対応する角度は、仮想直線K1が東西に延びる仮想直線であるとした場合に、角度θ1である。
【0170】
また、リンクの方位とは、リンクが延在する方向の方角を意味する。リンクが延在する方向とは、リンクに沿った2つの方向のうち、リンクに対応する道路において車両が走行可能な方向に対応する方向を意味する。また、リンクの方位に対応する角度とは、東に向かう方向を基準として、東に向かう方向と、リンクの方位との反時計回りの離間角度を意味する。
【0171】
図5の例では、リンクL1の方位は、方向Z1の方角である。また、リンクL1の方位に対応する角度は、仮想直線K2が東西に延びる仮想直線であるとした場合に、角度θ2である。また、図5の例では、リンクL2の方位は、方向Z2の方角である。また、リンクL2の方位に対応する角度は、仮想直線K3が東西に延びる仮想直線であるとした場合に、角度θ3である。
【0172】
図5の例において、車両とリンクL1との方位誤差は、角度θ1と角度θ2との差である。また、車両とリンクL2との方位誤差は、角度θ1と角度θ3との差である。つまり、図5の例において、式(18)では、δθは、角度θ1と角度θ2との差、または、角度θ1と角度θ3との差を示す。
【0173】
式(18)に示すように、評価量τは、δxを二乗した値にΔxを二乗した値で割ったものと、δyを二乗した値にΔyを二乗した値で割ったものと、δθを二乗した値にΔθを二乗した値で割ったものと、の和により算出される。Δxは、推定部22が推定した車両のx軸方向の位置の誤差を示す。つまり、Δxは、推定部22が推定した車両のx軸方向の位置の分散を示す。また、Δyは、推定部22が推定した車両のy軸方向の位置の誤差を示す。つまり、Δyは、推定部22が推定した車両のy軸方向の位置の分散を示す。また、Δθは、推定部22が推定した車両の方位の誤差を示す。つまり、Δθは、推定部22が推定した車両の方位の分散を示す。Δx、Δy、及び、Δθは、推定部22が推定した車両状態推定値の誤差共分散行列が含む分散である。このように、評価量は、車両の位置とリンクの位置との誤差を推定した車両の位置の誤差により無次元化した値と、車両の方位とリンクの方位との誤差を、推定した車両の位置の誤差により無次元化した値との和により算出される値である。
【0174】
図5の例では、制御部2は、マップマッチング候補リンクとして取得したリンクL1の評価量とリンクL2の評価量とを算出する。
【0175】
リンクL1の評価量をτ1とし、位置M1と交点MM1とのx座標の差をδx1とし、位置M1と交点MM1とのy座標の差をδy1とし、角度θ1と角度θ2との差をδθ1とした場合、評価量τ1は、下記に示す式(19)で表される。
【0176】
τ1=δx1/Δx+δy1/Δy+δθ1/Δθ・・・(19)
【0177】
一方、リンクL2の評価量をτ2とし、位置M1と交点MM2とのx座標の差をδx2とし、位置M1と交点MM2とのy座標の差をδy2とし、角度θ1と角度θ3との差をδθ2とした場合、評価量τ2は、下記に示す式(20)で表される。
【0178】
τ2=δx2/Δx+δy2/Δy+δθ2/Δθ・・・(20)
【0179】
図5の例では、距離l2より距離l1が小さい。すなわち、位置M1と交点MM2とにおけるx軸座標及びy座標の差より、位置M1と交点MM2とにおけるx座標及びy座標の差が小さい。また、図5の例では、角度θ1と角度θ3との差より角度θ1と角度θ2との差が小さい。このため、リンクL1の評価量であるτ1の値の方が、リンクL2の評価量であるτ2の値よりも小さくなる。したがって、制御部2は、評価量が小さいリンクL1を、車両の現在位置を対応付けるリンクとして決定する。
【0180】
このように、制御部2は、推定した車両の状態に基づいて、マップマッチングの対象となるリンクの評価を実行する。上述したように、制御部2は、リンクを評価する際、推定部22が推定した車両状態推定値の誤差共分散行列が含む分散に基づき、リンクを評価する。当該誤差共分散行列は、白色性の誤差としてGPS車両位置の観測誤差が入力されたカルマンゲインKに基づいて、算出された行列であり、精度よく算出された誤差を含む行列である。したがって、制御部2は、当該誤差共分散行列が含む分散をリンクの評価に用いることにより、正確にリンクを評価できる。
【0181】
以上、説明したように、ナビゲーション装置1(位置推定装置、車載装置)は、GPS信号を受信するGPS受信部5と、GPS受信部5が受信したGPS信号に基づくGPS車両位置(GPS測位位置)を含む観測量を観測する観測部21と、観測部21が観測する観測量とカルマンフィルタとに基づいて、車両の現在地に関する状態量を推定する推定部22と、を備える。推定部22は、車両状態予測値(状態量の予測値)と、車両状態予測値の誤差共分散行列(予測値の誤差)とを算出する。また、推定部22は、車両状態予測値と、車両状態予測値の誤差共分散行列と、観測部21が観測する観測量の誤差共分散行列(観測量の誤差)とに基づいて、車両状態推定値(状態量の推定値)と、車両状態推定値の誤差共分散行列(推定値の誤差)とを算出する。そして、推定部22は、車両状態推定値と、車両状態推定値の誤差共分散行列とを算出する際、あるタイミング(第1のタイミング)から、当該あるタイミングにおいて受信したGPS信号が、観測部21による前記GPS車両位置の観測に反映しない当該あるタイミングと異なる他のタイミング(第2のタイミング)までの期間に基づく重み付けを、GPS車両位置の測位誤差に付与する。
【0182】
GPS車両位置の測位誤差に当該期間に基づく重み付けを付与することにより、GPS車両位置の測位誤差は、白色性の誤差として見做せるGPS車両位置の観測誤差として設定される。したがって、白色性の誤差が前提となる観測量の誤差共分散行列に対してGPS車両位置の観測誤差を適切に入力でき、推定部22は、カルマンゲインKを正確に算出できる。そして、推定部22は、カルマンゲインKが正確に算出されるため、車両の現在地に関する状態を精度よく推定できる。
【0183】
また、推定部22は、車両状態推定値と、車両状態推定値の誤差共分散行列を算出する際、あるタイミングで発生したGPS受信影響(第1の影響)に基づく誤差(離間誤差)がGPS車両位置に対して発生してから、あるタイミングと異なる他のタイミングで発生した当該GPS受信影響と異なるGPS受信影響(第2の影響)に基づく誤差(離間誤差)がGPS車両位置に対して発生するまでの期間に基づく重み付けを、GPS車両位置の測位誤差に付与する。
【0184】
当該期間は、図4の場合、タイミングT1においてGPS受信影響が発生し、観測されるGPS車両位置に対して、当該GPS受信影響に基づく離間誤差が発生してから、タイミングT5において当該GPS受信影響と異なるGPS受信影響が発生し、この異なるGPS受信影響に基づく離間誤差がGPS車両位置に対して発生するまでの期間である。推定部22は、カルマンゲインKを算出する際、この期間に基づく重み付け(√(2τ))の付与により、この期間において有色性の誤差であるGPS車両位置の測位誤差を、白色性の誤差と見做せるGPS車両位置の観測誤差として観測量の誤差共分散行列(R)に入力し、カルマンゲインKを算出する。したがって、推定部22は、この期間における車両の現在地に関する状態を精度よく推定できる。
【0185】
また、推定部22は、車両状態推定値と、車両状態推定値の誤差共分散行列を算出する際、GPS受信影響に基づく誤差(離間誤差)がGPS車両位置に対して発生してから解消するまでの期間に基づく重み付けを、GPS車両位置の誤差に付与する。
【0186】
前述した通り、τが示す期間は、GPS受信影響が発生し、当該GPS受信影響に基づく離間誤差がGPS車両位置に対して発生してから、当該離間誤差が解消するまでの期間でもよい。推定部22は、このような期間であっても、この期間において有色性の誤差であるGPS車両位置の測位誤差を、白色性の誤差として見做せるGPS車両位置の観測誤差として観測量の誤差共分散行列(R)に入力し、カルマンゲインKを算出する。したがって、推定部22は、この期間における車両の現在地に関する状態を精度よく推定できる。
【0187】
上述した実施形態は、あくまでも本発明の一態様を例示するものであって、本発明の趣旨を逸脱しない範囲で任意に変形、及び応用が可能である。
【0188】
例えば、図1は、本願発明を理解容易にするために、ナビゲーション装置1の機能構成を主な処理内容に応じて分類して示した概略図であり、ナビゲーション装置1の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、1つの構成要素がさらに多くの処理を実行するように分類することもできる。
【0189】
また、例えば、図2、及び、図3のフローチャートの処理単位は、制御部2の処理を理解容易にするために、主な処理内容に応じて分割したものであり、処理単位の分割の仕方や名称によって、本発明が限定されることはない。制御部2の処理は、処理内容に応じて、さらに多くの処理単位に分割してもよい。また、1つの処理単位がさらに多くの処理を含むように分割してもよい。
【0190】
また、例えば、上述した実施形態では、位置推定装置を、車両に搭載される車載装置であるナビゲーション装置1として例示したが、位置推定装置の形態は任意であり、例えば歩行者が携帯するポータブル型の装置でも良い。
【符号の説明】
【0191】
1 ナビゲーション装置(位置推定装置、車載装置)
5 GPS受信部
21 観測部
22 推定部
図1
図2
図3
図4
図5