このシリーズの目標
このシリーズで作るのは、次のようなシステムです。
– GNSSとIMUを、ルーズカップリング構成のカルマンフィルタで統合し、
– 位置・速度・姿勢を出力する。
GNSS単独では更新レートが低く（数Hz）、トンネルや市街地で途絶します。一方でIMU単独は高レート（100Hz）で姿勢・速度・位置を出せますが、積分によって時間とともにドリフトします。両者を統合することで、「高レートで滑らか・短時間は正確（IMU）」かつ「長時間で発散しない・絶対位置を持つ（GNSS）」という、互いの弱点を補い合う航法解を得るのが狙いです。
第1回となる今回は、実装の細部に入る前に、アルゴリズム全体の構想設計を整理します。座標系・状態表現・フィルタ方式・データフロー・起動シーケンスといった、後続の実装すべてを支える土台を固めます。
なぜルーズカップリングか
GNSS/IMU統合には、結合の深さによっていくつかの方式があります。
ルーズカップリング（疎結合）は、GNSS受信機が内部で計算した「位置・速度の解」をそのまま観測値として使う方式です。受信機が出力するナビゲーション解を一段上のフィルタに渡すだけなので、実装が単純で、受信機をブラックボックスとして扱えます。
タイトカップリング（密結合）は、受信機内部の生の擬似距離・ドップラを観測に使う方式で、可視衛星が4個未満でも部分的に補正できるなどの利点がありますが、受信機内部へのアクセスと、より複雑な観測モデルが必要になります。
今回のシステムでは、まず動作する統合航法を素早く立ち上げ、設計の見通しを良くすることを優先し、ルーズカップリングを採用します。GNSS受信機が出す位置・速度解を観測として使い、生の擬似距離は使いません。
なぜ誤差状態カルマンフィルタ（ESKF）か
姿勢を含む航法状態は、本質的に非線形で、しかもクォータニオンのように「単純な足し算ができない」量を含みます。これを通常の拡張カルマンフィルタ（EKF）でそのまま扱うと、線形化の誤差が大きくなりがちです。
そこで採用するのが、誤差状態カルマンフィルタ（ESKF, Error-State Kalman Filter）です。ESKFは状態を二段構えで扱います。
– 公称状態（nominal state）：非線形な運動方程式で、そのまま高レートに伝播する。
– 誤差状態（error state）：公称状態からの小さなズレ。これだけをカルマンフィルタで線形に推定し、公称状態へ注入（補正）する。
誤差状態は常に原点付近の小さな量なので線形化が良く効き、姿勢誤差も3次元の微小回転ベクトルとして素直に表現できます。数値的に安定で、姿勢の扱いがきれいになるのがESKFの利点です。
座標系と姿勢表現
本システムは、すべての計算を ECEF（地球中心・地球固定）座標系で行います。地球とともに回転する直交座標系で、GNSSが出力する位置・速度とも相性が良く、重力・コリオリ・地球自転を一貫して扱えます。
姿勢は、機体座標系（Body）からECEFへの回転を表すクォータニオン \( \mathbf{q}_{be} \) で表現します。対応する回転行列を \( \mathbf{C}_b^e \)（Body→ECEF）、その転置を \( \mathbf{C}_e^b \) と書きます。
状態ベクトル
公称状態は、姿勢・速度・位置と、IMUの2種類のバイアスからなります。
$$\mathbf{x} = \{\,\mathbf{q}_{be},\ \mathbf{v}_e,\ \mathbf{r}_e,\ \mathbf{b}_a,\ \mathbf{b}_g\,\}$$
ここで \( \mathbf{v}_e \) はECEF速度、\( \mathbf{r}_e \) はECEF位置、\( \mathbf{b}_a \) は加速度計バイアス、\( \mathbf{b}_g \) はジャイロバイアスです。
カルマンフィルタが推定するのは、この公称状態からのズレ＝15次元の誤差状態です。
$$\delta\mathbf{x} = \begin{bmatrix} \delta\boldsymbol{\theta}_e \\ \delta\mathbf{v}_e \\ \delta\mathbf{r}_e \\ \delta\mathbf{b}_a \\ \delta\mathbf{b}_g \end{bmatrix} \in \mathbb{R}^{15}$$
それぞれ、姿勢誤差・速度誤差・位置誤差・加速度計バイアス誤差・ジャイロバイアス誤差です（各3次元）。姿勢誤差 \( \delta\boldsymbol{\theta}_e \) は微小回転ベクトルとして表します。
推定した誤差状態は、公称状態へ次のように注入して補正します。
$$\mathbf{q}_{be} \leftarrow \mathrm{Exp}(\delta\boldsymbol{\theta}_e) \otimes \mathbf{q}_{be},\quad \mathbf{v}_e \mathrel{+}= \delta\mathbf{v}_e,\quad \mathbf{r}_e \mathrel{+}= \delta\mathbf{r}_e,\quad \mathbf{b}_a \mathrel{+}= \delta\mathbf{b}_a,\quad \mathbf{b}_g \mathrel{+}= \delta\mathbf{b}_g$$
ここで \( \mathrm{Exp}(\cdot) \) は回転ベクトルからクォータニオンへの指数写像です。注入後は誤差状態をゼロにリセットします。
全体構成：予測と更新
ESKFは「予測」と「更新」の2ステップを繰り返します。予測はIMU由来で高レート（100Hz）に走り、更新はGNSSが届いたときだけ低レートで走ります。
予測ステップ（INSメカニゼーション + 共分散伝播）
IMU（ADIS16470）からは、速度増分 \( \Delta\mathbf{v} \) と角度増分 \( \Delta\boldsymbol{\theta} \) が得られます。これをサンプル時間 \( \Delta t \) で割り、バイアスを差し引いて比力と角速度を復元します。
$$\mathbf{f}_b = \Delta\mathbf{v}/\Delta t – \mathbf{b}_a,\qquad \boldsymbol{\omega}_{ib}^b = \Delta\boldsymbol{\theta}/\Delta t – \mathbf{b}_g$$
次に、機体の角速度から地球自転分を除いて、ECEF基準の角速度にします。
$$\boldsymbol{\omega}_{eb}^b = \boldsymbol{\omega}_{ib}^b – \mathbf{C}_e^b\,\boldsymbol{\omega}_{ie}^e$$
これで姿勢を伝播します。
$$\mathbf{q}_{be} \leftarrow \mathbf{q}_{be} \otimes \mathrm{Exp}(\boldsymbol{\omega}_{eb}^b\,\Delta t)$$
比力をECEFへ変換し、重力とコリオリ項を足してECEF加速度を作ります。
$$\mathbf{a}_e = \mathbf{f}_e + \mathbf{g}_e – 2\,\boldsymbol{\omega}_{ie}^e \times \mathbf{v}_e,\qquad \mathbf{f}_e = \mathbf{C}_b^e\,\mathbf{f}_b$$
あとは運動学を積分して速度・位置を更新します（実装では台形則で1次精度を上げています）。
$$\mathbf{v}_e \leftarrow \mathbf{v}_e + \mathbf{a}_e\,\Delta t,\qquad \mathbf{r}_e \leftarrow \mathbf{r}_e + \mathbf{v}_e\,\Delta t$$
公称状態の伝播と並行して、誤差状態の共分散 \( \mathbf{P} \) も伝播します。連続時間のシステム行列 \( \mathbf{F} \)（15×15）を組み、離散化して状態遷移行列 \( \boldsymbol{\Phi} \) を作ります。
$$\boldsymbol{\Phi} = \mathbf{I} + \mathbf{F}\,\Delta t,\qquad \mathbf{P}^- = \boldsymbol{\Phi}\,\mathbf{P}\,\boldsymbol{\Phi}^\top + \mathbf{Q}_d$$
プロセスノイズ \( \mathbf{Q}_d \) はIMUのノイズ特性（データシート由来）から決めます。本システムではIMUを相対的に強く信頼する配分にしています。
更新ステップ（GNSS・ルーズカップリング）
GNSSが位置・速度解を出したら、それを観測として更新します。アンテナとIMUの取り付け位置の差（レバーアーム \( \mathbf{L} \)）を補正した観測モデルは次のとおりです。
$$\mathbf{z} = \begin{bmatrix} \mathbf{r}_e + \mathbf{C}_b^e\,\mathbf{L} \\ \mathbf{v}_e + \mathbf{C}_b^e(\boldsymbol{\omega}_{eb}^b \times \mathbf{L}) \end{bmatrix}$$
イノベーション（観測残差）は、実測とモデル予測の差です。
$$\mathbf{y} = \mathbf{z}_{\text{meas}} – h(\mathbf{x})$$
カルマンゲインを計算し、誤差状態を求めて公称状態へ注入します。
$$\mathbf{S} = \mathbf{H}\,\mathbf{P}\,\mathbf{H}^\top + \mathbf{R},\qquad \mathbf{K} = \mathbf{P}\,\mathbf{H}^\top\,\mathbf{S}^{-1},\qquad \delta\mathbf{x} = \mathbf{K}\,\mathbf{y}$$
ここで観測行列 \( \mathbf{H} \) は6×15で、レバーアーム項を含みます。
実環境では、マルチパスや誤エポックによる外れ値が混入します。これを素通しさせないために、正規化イノベーション二乗（NIS）でゲートをかけ、しきい値を超える観測は棄却します。
$$\mathbf{y}^\top\,\mathbf{S}^{-1}\,\mathbf{y} > \gamma \ \Rightarrow\ \text{この観測を棄却}$$
加えて、\( \mathbf{S} \) が特異な場合や \( \delta\mathbf{x} \) にNaN/Infが現れた場合も棄却し、1つの異常値が共分散を恒久的に汚染するのを防ぎます。最後に共分散を対称化し、バイアス推定値には飽和制限をかけて数値ロバスト性を確保します。
ハードウェア構成
ハードウェアは、リアルタイム処理とセンサフュージョンを担う1つのCPUを中心としたハブ構成です。CPUの周りに、絶対位置を与えるGNSS系と、高レートで運動を捉える慣性・磁気センサ系が接続されます。
役割で大きく分けると、次の3系統になります。
– GNSS系：アンテナ・CLAS受信機・GNSSモジュール。低レートだが絶対的な位置・速度を与える。
– 慣性・磁気センサ系：IMUとMAGセンサ。高レートで加速度・角速度・方位を与える。
– 演算部：CPU。各センサを読み取り、これまで述べたフィルタで統合して航法解を出力する。
GNSSは数Hz程度で更新され配信遅延もありますが絶対位置を持ちます。一方IMUは100Hzで滑らかに運動を捉えますが積分ドリフトを持ちます。この相補的な2系統をCPUで結びつけるのが、ハードウェア構成の基本的な考え方です。
● GNSSアンテナ
L1／L2／L6の各帯に対応したアンテナです。測位に使う通常のGNSS信号（L1／L2など）に加えて、高精度補強に使うL6帯も受信できることがポイントです。
● CLAS受信機
アンテナのRFはまずCLAS受信機に入り、ここでL6帯の補強信号を復調します。CLAS受信機は、受け取ったRFをGNSSモジュールへパススルーしつつ、復調して得たL6補正データをGNSSモジュールへ供給します。
● GNSSモジュール
マルチバンドのGNSS受信機で、CLAS受信機から渡されるL6補正を使って高精度な測位解（位置・速度）を計算します。本システムはルーズカップリングなので、ここで計算された測位解そのものを観測としてCPUへ渡します。生の擬似距離は使いません。
GNSSモジュールはもう1つ重要な出力を持ちます。1PPS（1パルス毎秒）の時刻同期信号です。これについては後述の時刻同期の項で説明します。
● IMU
ジャイロと加速度計を備えた6軸のMEMS慣性計測ユニットです。CPUへはSPIで接続し、新しいサンプルが取得できたことを知らせるデータレディ（DR）信号で割り込みをかけます。
このIMUは、生の角速度・加速度ではなく、サンプル区間で積分済みの角度増分（Δθ）と速度増分（ΔV）を出力できます。予測ステップで入力として使ったのが、まさにこのΔθとΔVです。積分済みの増分を使うことで、サンプル間の運動を取りこぼしにくくなります。
● MAGセンサ
3軸の地磁気センサで、CPUへはSPIで接続します。主にヨー（方位）の観測に使い、静止時のヘディング拘束など、姿勢推定の安定化に寄与します。
● 演算部（CPU）
CPUは、IMU・GNSS・地磁気の3系統を読み取り、これまで述べたフィルタで統合して位置・速度・姿勢を推定します。予測ループは100Hzで回るため、SPIの読み取り、割り込み処理、行列演算をこのレートで滞りなくこなせる処理能力が求められます。出力段では、ECEFの内部状態を緯度経度高度（LLH）や姿勢へ変換して送信します。
● インターフェースと信号の流れ
各デバイスの接続を信号の流れに沿って整理すると、次のようになります。
– アンテナ → CLAS受信機：RF（L1／L2／L6）。
– CLAS受信機 → GNSSモジュール：RFパススルーと、復調したL6補正データ。
– GNSSモジュール → CPU：UART。位置・速度・時刻の航法データを送る。
– GNSSモジュール → CPU／IMU：1PPSの時刻同期パルス。
– IMU ↔ CPU：SPI。DR信号で新サンプルを通知。
– MAGセンサ ↔ CPU：SPI。
GNSS系はRFとUARTという低レート・低頻度の配線で済み、慣性・磁気センサ系は高レートのSPIでCPUに直結する、という役割分担になっています。
ハードウェア構成のまとめ
ハードウェア構成を一言でまとめると、CPUを中心に、絶対位置を与えるGNSS系と高レートで運動を捉える慣性・磁気センサ系を、用途に応じたインターフェースで束ねた構成です。アンテナからCLAS受信機を経てGNSSモジュールへ至るRF経路、UARTでの航法データ受け渡し、SPIでのセンサ読み取り、そして1PPSによる時刻同期が、本システムのアルゴリズムを動かすための土台になっています。
製作した試作機