線形カルマンフィルタによる三次元位置推定

移動体の位置推定を行う代表的な手法として,加速度を積分して得られた位置・速度(これをデッドレコニングといいます)と,GNSSにより得られた位置・速度をカルマンフィルタにより組み合わせて正しい位置・速度を推定する方法があります.

そこで,本記事では上述したカルマンフィルタによる位置推定シミュレーションを行い,その結果を示します.ただし,カルマンフィルタの理論的な部分については言及しませんので,詳しくは参考文献を参照して下さい.

シミュレーションのために


基本的に参考文献[2]の式をそのまま実装します.6ページとコンパクトにまとまった文献なので,目を通してみて下さい.位置推定の部分だけなら3ページもありません.

また,直線に動かした場合のシミュレーションをしてもあまり面白くないので,今回は慣性センサを螺旋状に動かした場合のシミュレーションを行います.

時刻tを媒介変数とする螺旋の式は以下で表されます.

螺旋の式

この式を時間微分することで,各時刻における速度と加速度の真値が得られます.シミュレーションでは,上式により得られた位置・速度・加速度の真値に正規分布に従う乱数を乗せることで,実際の計測値を再現します.

実際に自動車やドローンに慣性センサを搭載して位置推定を行う際には,まず加速度の座標変換を行う必要があります.とはいえ,座標変換まで考えてシミュレーションするとややこしくなるので,今回は既に座標変換された加速度が得られるものとします.

実装


最近はRustをよく使っているので,シミュレーション計算はRustで行い,シミュレーション結果の可視化はPythonで行うことにします.

また,RustからPythonにデータを渡す際は一度CSVファイルを経由します.こうすることでRustとPythonのプログラムを分離できて扱いやすくなり,計算結果を保存しておくことができます.

特に難しい書き方はしていないので,Rustを使ったことがなくても処理の流れは大体理解できると思います.

Rustで行列演算を行うためにnalgebraを使用しています.nalgebraは行列型をnew()で作れるはずなのですが,なぜかnew()が見つからないと言われたので,一旦zeros()で零行列を作ってから各行列要素を書き換える実装としました.

[dependencies]
nalgebra = "0.24"
rand = "0.6"
//! カルマンフィルタによる三次元位置推定シミュレーション
//! シミュレーション結果をresult.csvファイルに出力する.

use std::fs;
use std::io::{Write, BufWriter};
use nalgebra as na;
use na::{U3, U6, U9, Matrix, ArrayStorage, VectorN, Vector6};
use rand::distributions::{Distribution, Normal};

type Vector9 = VectorN<f64, U9>;
type Matrix6x6 = Matrix<f64, U6, U6, ArrayStorage<f64, U6, U6>>;
type Matrix6x9 = Matrix<f64, U6, U9, ArrayStorage<f64, U6, U9>>;
type Matrix9x3 = Matrix<f64, U9, U3, ArrayStorage<f64, U9, U3>>;
type Matrix9x9 = Matrix<f64, U9, U9, ArrayStorage<f64, U9, U9>>;

const DT: f64 = 0.001;    // デッドレコニングの計算周期
const N: usize = 20000;  // 計算ステップ数

// 螺旋の定数a, b
const SPIRAL_A: f64 = 1.0;
const SPIRAL_B: f64 = 1.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::<f64>::zeros();

    #[allow(non_snake_case)]
    let mut H = Matrix6x9::zeros();
    for i in 0..3 {
        H[(i, i)] = 1.0;
        H[(i+3, i+3)] = 1.0;
    }

    // 共分散行列
    #[allow(non_snake_case)]
    let mut P = Matrix9x9::zeros();
    for i in 0..3 {
        P[(i, i)] = 1000.0;
    }

    // 加速度センサのノイズ分散
    #[allow(non_snake_case)]
    let mut Q = Matrix9x9::zeros();
    Q[(6, 6)] = 0.0001;
    Q[(7, 7)] = 0.0001;
    Q[(8, 8)] = 0.0001;

    // GNSSによる計測値の分散
    #[allow(non_snake_case)]
    let mut R = Matrix6x6::zeros();
    // 位置の分散
    R[(0, 0)] = 0.2;  // x
    R[(1, 1)] = 0.2;  // y
    R[(2, 2)] = 0.2;  // z
    // 速度の分散
    R[(3, 3)] = 0.01;  // x
    R[(4, 4)] = 0.01;  // y
    R[(5, 5)] = 0.01;  // z

    // 9x9単位行列
    let mut eye9x9 = Matrix9x9::zeros();
    for i in 0..9 {
        eye9x9[(i, i)] = 1.0;
    }

    let mut x_dead = x;  // デッドレコニングのみの値
    let mut t: f64 = 0.0;
    for i in 0..N {
        t += DT;

        // 加速度を更新
        // 慣性座標系上の加速度(座標変換は考えない).
        x[6] = -SPIRAL_A * t.sin() + randn.sample(&mut rand::thread_rng()) * Q[(6, 6)];
        x[7] = -SPIRAL_A * t.cos() + randn.sample(&mut rand::thread_rng()) * Q[(7, 7)];
        x[8] = randn.sample(&mut rand::thread_rng()) * Q[(8, 8)];

        // デッドレコニング
        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() + Q * (B * 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();
        // ------------------------------------ //

        // GNSSデータの更新レートは5Hz
        if i % 200 == 0 {
            // GNSSから得た位置・速度を更新(真値にノイズを乗せる)
            // 位置
            z[0] = SPIRAL_A * t.sin() + randn.sample(&mut rand::thread_rng()) * R[(0, 0)];
            z[1] = SPIRAL_A * t.cos() + randn.sample(&mut rand::thread_rng()) * R[(1, 1)];
            z[2] = SPIRAL_B * t + randn.sample(&mut rand::thread_rng()) * R[(2, 2)];
            // 速度(ドップラー効果を利用して得られる)
            z[3] = SPIRAL_A * t.cos() + randn.sample(&mut rand::thread_rng()) * R[(3, 3)];
            z[4] = -SPIRAL_A * t.sin() + randn.sample(&mut rand::thread_rng()) * R[(4, 4)];
            z[5] = SPIRAL_B + randn.sample(&mut rand::thread_rng()) * R[(5, 5)];

            // フィルタリングステップ
            let g = (P * H.transpose()) * (H * P * H.transpose() + R).try_inverse().unwrap();
            x = x + g * (z - H * x);
            P = (eye9x9 - g * H) * 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により得られた位置情報は結構バラついていますが,カルマンフィルタを用いることでここまで綺麗に推定できるのは凄いですね.

今回は逐次計算を行う実装としましたが,線形時不変のモデルなのでカルマンゲインはある値に収束し,これは定常カルマンゲインとしてオフラインで計算することが出来ます.なので,マイコンなどに組み込む際は事前に定常カルマンゲインを求めておくことで計算量を大幅に削減できます.

参考文献

  1. Python 数値計算入門,”[Matplotlib] 3次元データの可視化” (https://python.atelierkobato.com/axes3d/)
  2. Gabriel Schmitz & Tiago Alves & Renato Henriques & Edison Freitas & Ebrahim El’Youssef . (2016). A simplified approach to motion estimation in a UAV using two filters, IFAC-PapersOnline, Pages325-330
  3. 足立 修一,丸太 一郎,”カルマンフィルタの基礎”,東京電機大学出版局(2012)

コメントを残す