移動体の位置推定を行う代表的な手法として，加速度を積分して得られた位置・速度（これをデッドレコニングといいます）と，GNSSにより得られた位置・速度をカルマンフィルタにより組み合わせて正しい位置・速度を推定する方法があります．
そこで，本記事では上述したカルマンフィルタによる位置推定シミュレーションを行い，その結果を示します．ただし，カルマンフィルタの理論的な部分については言及しませんので，詳しくは参考文献を参照して下さい．
シミュレーションのために
基本的に参考文献[2]の式をそのまま実装します．6ページとコンパクトにまとまった文献なので，目を通してみて下さい．位置推定の部分だけなら3ページもありません．
また，直線に動かした場合のシミュレーションをしてもあまり面白くないので，今回は慣性センサを螺旋状に動かした場合のシミュレーションを行います．
時刻tを媒介変数とする螺旋の式は以下で表されます．
$[ \begin{align*} x &= a \sin (t) \\ y &= a \cos (t) \\ z &= b \, t \end{align*} $]この式を時間微分することで，各時刻における速度と加速度の真値が得られます．シミュレーションでは，上式により得られた位置・速度・加速度の真値に正規分布に従う乱数を乗せることで，実際の計測値を再現します．
実際に自動車やドローンに慣性センサを搭載して位置推定を行う際には，まず加速度の座標変換を行う必要があります．とはいえ，座標変換まで考えてシミュレーションするとややこしくなるので，今回は既に座標変換された加速度が得られるものとします．
実装
最近はRustをよく使っているので，シミュレーション計算はRustで行い，シミュレーション結果の可視化はPythonで行うことにします．
また，RustからPythonにデータを渡す際は一度CSVファイルを経由します．こうすることでRustとPythonのプログラムを分離できて扱いやすくなり，計算結果を保存しておくことができます．
特に難しい書き方はしていないので，Rustを使ったことがなくても処理の流れは大体理解できると思います．
※追記(2021/11/19)：nalgebraのバージョンを0.24から0.29に上げました．また，行列Qのサイズも修正しましたが，これによる計算結果への影響はありません．ただし，螺旋とノイズのパラメータを変更したので計算結果のグラフも変更してあります．
[dependencies]
nalgebra = "0.29"
rand = "0.6"
//! カルマンフィルタによる三次元位置推定シミュレーション
//! シミュレーション結果はresult.csvに保存される．
use std::fs;
use std::io::{Write, BufWriter};
use nalgebra::{SVector, SMatrix};
use rand::distributions::{Distribution, Normal};
type Vector6 = SVector<f64, 6>;
type Vector9 = SVector<f64, 9>;
type Matrix3x3 = SMatrix<f64, 3, 3>;
type Matrix6x6 = SMatrix<f64, 6, 6>;
type Matrix6x9 = SMatrix<f64, 6, 9>;
type Matrix9x3 = SMatrix<f64, 9, 3>;
type Matrix9x9 = SMatrix<f64, 9, 9>;
const DT: f64 = 0.001; // デッドレコニングの計算周期
const N: usize = 20000; // 計算回数
const GNSS_RATE: usize = 10; // GNSSの更新レート[Hz]
// GNSSの更新周期[サンプル]
const GNSS_PERIOD: usize = (1.0 / (GNSS_RATE as f64 * DT)) as usize;
// 螺旋の定数a, b
const SPIRAL_A: f64 = 20.0;
const SPIRAL_B: f64 = 3.0;
fn main() {
// CSVファイルにデータ保存
// 同一ファイルが存在したら上書き
let mut file = BufWriter::new( fs::File::create("result.csv").unwrap() );
// 乱数を生成
let randn = Normal::new(0.0, 1.0); // 平均値:0，標準偏差:1
// 状態量ベクトル
let mut x = Vector9::zeros();
x[1] = SPIRAL_A;
x[3] = SPIRAL_A;
x[5] = SPIRAL_B;
x[7] = -SPIRAL_A;
// システム行列
#[allow(non_snake_case)]
let mut A = Matrix9x9::zeros();
for i in 0..3 {
A[(i, i)] = 1.0;
A[(i, i+3)] = DT;
A[(i, i+6)] = 0.5 * DT;
A[(i+3, i+3)] = 1.0;
A[(i+3, i+6)] = DT;
A[(i+6, i+6)] = 1.0;
}
// 入力行列
#[allow(non_snake_case)]
let mut B = Matrix9x3::zeros();
for i in 0..3 {
B[(i+6, i)] = 1.0;
}
// 観測値ベクトル
let mut z = Vector6::zeros();
// 出力行列
#[allow(non_snake_case)]
let mut C = Matrix6x9::zeros();
for i in 0..3 {
C[(i, i)] = 1.0;
C[(i+3, i+3)] = 1.0;
}
// 共分散行列
#[allow(non_snake_case)]
let mut P = 1000.0 * Matrix9x9::identity();
// 加速度センサのノイズ分散
#[allow(non_snake_case)]
let Q = 0.0001 * Matrix3x3::identity();
// GNSSによる計測値の分散
let p_r = 10.0; // 位置の分散
let v_r = 0.5; // 速度の分散
#[allow(non_snake_case)]
let R = Matrix6x6::from_diagonal(&Vector6::new(p_r, p_r, p_r, v_r, v_r, v_r));
// 単位行列
let eye9x9 = Matrix9x9::identity();
let mut x_dead = x; // デッドレコニングの値
for i in 0..N {
let t = DT * i as f64;
// 加速度を更新
// 慣性座標系上の加速度（座標変換は考えない）．
x[6] = -SPIRAL_A * t.sin() + randn.sample(&mut rand::thread_rng()) * Q[(0, 0)].sqrt();
x[7] = -SPIRAL_A * t.cos() + randn.sample(&mut rand::thread_rng()) * Q[(1, 1)].sqrt();
x[8] = randn.sample(&mut rand::thread_rng()) * Q[(2, 2)].sqrt();
// デッドレコニング
x_dead[6] = x[6];
x_dead[7] = x[7];
x_dead[8] = x[8];
x_dead = A * x_dead;
// 予測ステップ
x = A * x;
P = A * P * A.transpose() + B * Q * B.transpose();
// ---------- データ書き込み ---------- //
// 真値
file.write( format!("{:.7},{:.7},{:.7},", SPIRAL_A*t.sin(), SPIRAL_A*t.cos(), SPIRAL_B*t).as_bytes() ).unwrap();
// 位置推定値
file.write( format!("{:.7},{:.7},{:.7},", x[0], x[1], x[2]).as_bytes() ).unwrap();
// デッドレコニングのみの位置
file.write( format!("{:.7},{:.7},{:.7}", x_dead[0], x_dead[1], x_dead[2]).as_bytes() ).unwrap();
// ------------------------------------ //
if i % GNSS_PERIOD == 0 {
// GNSSから得た位置・速度を更新（真値にノイズを乗せる）
// 位置
z[0] = SPIRAL_A * t.sin() + randn.sample(&mut rand::thread_rng()) * R[(0, 0)].sqrt();
z[1] = SPIRAL_A * t.cos() + randn.sample(&mut rand::thread_rng()) * R[(1, 1)].sqrt();
z[2] = SPIRAL_B * t + randn.sample(&mut rand::thread_rng()) * R[(2, 2)].sqrt();
// 速度
z[3] = SPIRAL_A * t.cos() + randn.sample(&mut rand::thread_rng()) * R[(3, 3)].sqrt();
z[4] = -SPIRAL_A * t.sin() + randn.sample(&mut rand::thread_rng()) * R[(4, 4)].sqrt();
z[5] = SPIRAL_B + randn.sample(&mut rand::thread_rng()) * R[(5, 5)].sqrt();
// フィルタリングステップ
let g = (P * C.transpose()) * (C * P * C.transpose() + R).try_inverse().unwrap();
x = x + g * (z - C * x);
P = (eye9x9 - g * C) * P;
// ---------- データ書き込み ---------- //
// 観測値
file.write( format!(",{:.7},{:.7},{:.7}\n", z[0], z[1], z[2]).as_bytes() ).unwrap();
// ------------------------------------ //
} else {
// ---------- データ書き込み ---------- //
// 改行のみ
file.write(b"\n").unwrap();
// ------------------------------------ //
}
}
}
# Rustのプログラムで作成したresult.csvからmatplotlibでグラフを作成する．
import csv
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# Figureを追加
fig = plt.figure(figsize = (8, 8))
# 3DAxesを追加
ax = fig.add_subplot(111, projection='3d')
# Axesのタイトルを設定
ax.set_title("KF estimation", size = 20)
# 軸ラベルを設定
ax.set_xlabel("x", size = 14)
ax.set_ylabel("y", size = 14)
ax.set_zlabel("z", size = 14)
# 真値
ref_x = np.array([])
ref_y = np.array([])
ref_z = np.array([])
# 位置推定値
est_x = np.array([])
est_y = np.array([])
est_z = np.array([])
# デッドレコニング
drk_x = np.array([])
drk_y = np.array([])
drk_z = np.array([])
# 観測値
obs_x = np.array([])
obs_y = np.array([])
obs_z = np.array([])
# CSVからデータを読み出してnumpy配列に追加
with open('./result.csv') as f:
reader = csv.reader(f)
for row in reader:
nums = [float(v) for v in row] # 文字列から浮動小数点数に変換
# 真値
ref_x = np.append(ref_x, nums[0])
ref_y = np.append(ref_y, nums[1])
ref_z = np.append(ref_z, nums[2])
# 位置推定値
est_x = np.append(est_x, nums[3])
est_y = np.append(est_y, nums[4])
est_z = np.append(est_z, nums[5])
# デッドレコニングのみ
drk_x = np.append(drk_x, nums[6])
drk_y = np.append(drk_y, nums[7])
drk_z = np.append(drk_z, nums[8])
if len(row) == 12:
# 観測値
obs_x = np.append(obs_x, nums[9])
obs_y = np.append(obs_y, nums[10])
obs_z = np.append(obs_z, nums[11])
# 描画
ax.plot(ref_x, ref_y, ref_z, label="Reference", color="black") # 真値
ax.plot(drk_x, drk_y, drk_z, label="Dead Reckoning", color="red") # デッドレコニング
ax.plot(est_x, est_y, est_z, label="KF", color="blue") # 推定値
ax.plot(obs_x, obs_y, obs_z, label="GNSS", color="green", marker = ".", linestyle='None') # 観測値
plt.legend()
plt.show()
結果
上図がシミュレーション結果です．デッドレコニングのみの場合には運動の軌跡が真値から大幅にズレていますが，カルマンフィルタを用いることで，若干の位相遅れはあるもののかなり正確に推定できていることが確認できます．
GNSSにより得られた位置情報は結構バラついていますが，カルマンフィルタを用いることでここまで綺麗に推定できるのは凄いですね．
今回は逐次計算を行う実装としましたが，線形時不変のモデルなのでカルマンゲインはある値に収束し，これは定常カルマンフィルタのゲインとしてオフラインで計算することが出来ます．なので，マイコンなどに組み込む際は事前にゲインを求めておくことで計算量を大幅に削減できます．