カルマンフィルタを使ってマスターズ選手権2024決勝のA問題を解いてみた

はじめに

こんにちは、競技プログラミングのコンテストに参加している yunixです。
この記事では、去年のマスターズ選手権の A 問題をカルマンフィルタを使って解いてみようと思います。

なぜいまさら去年のマスターズ決勝をテーマにした記事を書いているかというと、

• A 問題で当日トップだったチームがカルマンフィルタを使っているという話は Twitter で聞いていた
• カルマンフィルタがどういう問題を解くために使えるかいまいちわかっていなかった
• カルマンフィルタを使った解法を解説している記事はなかった(ような気がする)

という事情があります。要するに復習を一年以上サボっていたのですね。
重い腰を上げてこの本で履修をしました。

この記事では、将来カルマンフィルタを使える問題が出た時のために、

• カルマンフィルタがどのような悩みを解決するか
• マスターズの A 問題ではどのようにしてカルマンフィルタを使用するか

を備忘録として残せればと思っています。記述の厳密性にはこだわらずに理解したことを書いていこうと思います(というかこだわるだけ理解が深くない)。

また、同じ悩み事を解決するための手段としては、粒子フィルタを使用した eijirou さんの解説があります。
粒子フィルタの方が C 問題に応用しやすく、マスターズで使うには便利であると思います。

マスターズ 2024 決勝ってどんな問題だったっけ...?

何せ 1 年以上前の話なので忘れてしまった方も多いと思います。簡単に記述すると以下のような問題でした:

• 目的：2 次元平面上でドローンを操作し、10 個の目的地を全て訪れる。
• アクション：毎ターン、「加速」または「壁距離計測」を選択。
  • 加速：速度変更（加速度制約あり）。
  • 計測：指定方向の壁までの距離を誤差付きで取得。
• 環境要因：
  • 飛行区域は壁で囲まれている（問題により内部に壁が存在）。
  • 操作後に風の影響で速度がランダムに変化。
• 移動と判定：
  • 速度に応じて位置更新。
  • 壁に衝突すると速度ゼロになり位置更新なし。
  • 移動経路が目的地に近づくと到達。
• スコア：ターン経過や壁衝突で減点、目的地到達で加点。5000 ターン以内の最高スコアを競う。

A 問題、B 問題、C 問題の主な違い（パラメータの傾向）：

• A 問題：
  • 内壁：なし
  • 風の影響（$\epsilon$）：大きい
  • 計測誤差（$\delta$）：そこそこ
• B 問題：
  • 内壁：多い（10 個固定）
  • 風の影響（$\epsilon$）：ほぼ無い
  • 計測誤差（$\delta$）：非常に小さい
• C 問題：
  • 内壁：ランダム（1〜10 個）
  • 風の影響（$\epsilon$）：大きい
  • 計測誤差（$\delta$）：そこそこ

この中で、今回は A 問題を対象に解説を書いていこうと思います。
A 問題は測定した壁からの距離の情報を取り入れて、現在の位置や速度を推定することがポイントの問題で、カルマンフィルタの応用先としては適しているためです。

一方で、B 問題では状態の推定が特に必要なく、C 問題では内壁があるために話がややこしくなります。特に C 問題を解くならば粒子フィルタの方が向いていると思います(A 問題でも粒子フィルタを使うのは良い)。

カルマンフィルタとは? 何が嬉しいか?

以下のような状況を考えます:

• ある系が時間発展している
• 時間発展の過程で、外部から入力を入れて、時間発展をコントロールできる
• その系の情報に関して観測できる情報は断片的だったり、状態を直接測れなかったりする
• 系の時間発展や、観測できる情報にはノイズが入る

カルマンフィルタは、このような時間発展や観測にノイズが入っていたり、状態を直接測れないようなときに、状態をいい感じに推定する手法です。

話をより詳しくするために、数式を使用して記述をします。
数式に使う変数は以下のように定義します:

• $x(t)$: 時刻 t における系の状態を表すベクトル
• $y(t)$: 時刻 t における観測値のベクトル
• $u(t)$: 時刻 t における状態を制御するための入力のベクトル(制御入力)
• $w(t)$: 時刻 t における状態の時間発展に対するノイズのベクトル(システムノイズ)、共分散行列 W の白色雑音
• $v(t)$: 時刻 t における観測値に対するノイズのベクトル(観測ノイズ)、共分散行列 V の白色雑音

このとき、系の状態の時間発展・観測値と状態間の関係を、離散時間の線形状態空間モデルで表現します:

$$x(t+1) = F(t) x(t) + G(t) u(t) + B(t) w(t)$$

$$y(t) = H(t) x(t) + v(t)$$

ここで、$F(t)$ は状態遷移行列、$G(t)$ は制御入力行列、$B(t)$ はシステムノイズ入力行列、$H(t)$ は観測行列です。
(ただし、マスターズの問題ではこれらの行列は時間に対して不変なので、この問題では固定値で大丈夫です)

カルマンフィルタは

• 系の時間発展や状態の観測を上記のような線形モデルで表現したときに、
• 観測した y(t)から逐次的に x(t)およびその共分散行列を推定する

ような手法です。

去年のマスターズ決勝の問題設定では

• 線形のモデルで正確に系の時間発展を記述できる
• 位置の推定は誤差付き: 観測の方程式の誤差として扱うことができる
• ランダムな風が吹いて速度が変わる: 状態方程式のノイズとして扱うことができる
• 現在の位置や速度を精度よく推定できると嬉しい

という事情がありました。
問題設定を状態空間モデルで表現しさえすれば、カルマンフィルタを使って現在の位置や速度をいい感じに推定でき、ドローンをコントロールする上で必要な情報を得ることができることになります。

この時点でカルマンフィルタを使うと良さそうな感じがしてきますね。

状態の推定方法

カルマンフィルタでは、以下のようなやり方で逐次的に観測値を取り入れて、状態とその誤差の共分散行列を更新していきます。

まず、時刻 $t=0$ における状態 $x(0)$ とその誤差の共分散行列 $P(0)$ を用意します。これは通常、問題設定や事前知識に基づいて決定されます。

そして、時刻 $t=0, 1, 2, \dots$ に対して、以下の予測ステップと更新ステップを繰り返します。

1. 予測ステップ

時刻 $t$ の状態の推定値 $\hat{x}(t|t)$ とその共分散行列 $P(t|t)$ から、時刻 $t+1$ の状態および共分散行列を予測します。

• 状態の予測:

  $$\hat{x}(t+1|t) = F(t) \hat{x}(t|t) + G(t) u(t)$$

• 共分散行列の予測:

  $$P(t+1|t) = F(t) P(t|t) F(t)^T + B(t) W(t) B(t)^T$$

  ここで、$W(t)$ はプロセスノイズ $w(t)$ の共分散行列である

ここで、$\hat{x}(t+1|t)$や$P(t+1|t)$のような条件付き確率記法を使用しています。
これは「時刻 t までに得られた情報を元にして、時刻 t+1 の状態を推定した値」であることを表現しています。

この予測ステップでは時刻 t+1 の観測情報はまだ取り込まれておらず、次の更新ステップによって$\hat{x}(t+1|t+1)$や$P(t+1|t+1)$を得ることになります。

2. 更新ステップ

時刻 $t+1$ で観測値 $y(t+1)$ が得られたら、予測した状態 $\hat{x}(t+1|t)$ と共分散行列 $P(t+1|t)$ を用いて、状態の推定値を更新します。

• カルマンゲインの計算:

  $$K(t+1) = \frac{P(t+1|t) H(t+1)^T} {(H(t+1) P(t+1|t) H(t+1)^T + V(t+1))}$$

  ここで、$V(t+1)$ は観測ノイズ $v(t+1)$ の共分散行列である

• 状態の更新:

  $$\hat{x}(t+1|t+1) = \hat{x}(t+1|t) + K(t+1) (y(t+1) - H(t+1) \hat{x}(t+1|t))$$

• 共分散行列の更新:

  $$P(t+1|t+1) = (I - K(t+1) H(t+1)) P(t+1|t)$$

  ここで、$I$ は単位行列である

これらのステップを繰り返すことで、観測データを取り込みながら、逐次的に状態を推定していくことができます。

この更新のステップの面白いところとしては、$\hat{x}(t+1|t+1)$は

• 時刻 t の状態から予測される値$\hat{x}(t+1|t)$
• 時刻 t+1 の観測値$y(t+1)$と、時刻 t の状態から予測される観測値 $H(t+1) \hat{x}(t+1|t)$の差

のカルマンゲイン K(t+1)による内分点になっていることです。
カルマンゲインの大小によって観測値とそれまでの状態のどちらを重視するかが決まっていると理解できます。

また、カルマンゲイン自体の式を見てみると、大雑把に言って

• 状態の誤差の共分散行列 $P(t+1|t)$
• 観測ノイズの共分散行列 $V(t+1)$

の大小を比較するようになっています。

• 状態の誤差が小さい場合にはカルマンゲインは 0 に近づく -> 元々の状態の情報が重視される
• 観測ノイズの方が小さい場合にはカルマンゲインは大きな値になる -> 観測の情報が重視される

という形で、状態と観測の不確定性をうまくバランスをとって新しい状態を推定していることがわかります。

なお、カルマンフィルタによる状態の予測としては、以下のような特徴が知られています:

• 線形システムの最適推定：システムが線形であり、ノイズがガウス分布に従うという仮定のもとでは、カルマンフィルタは状態の最小平均二乗誤差 (MMSE) 推定量を逐次的に計算する。つまり、他のどんな線形推定器よりも平均的に誤差が小さい、理論的に最適な推定結果を与える。
• 不確実性の定量化：状態の推定値だけでなく、その推定値がどれくらい確かであるかを示す共分散行列も同時に更新していく。これにより、推定結果の信頼度を把握できる
• 逐次処理：観測データが得られるたびに、前の推定結果を利用して新しい推定値を更新するため、全てのデータを一度に処理する必要がありません。リアルタイムシステムに適している

マスターズ A 問題の状態空間モデリング

最終的なコードはこちらです。
475,780 点と、本番トップの 440,152 点と比較しても良い点数が取れていて、カルマンフィルタによって状態をいい感じに推定できているのではないかと思います。

A 問題の seed=1 をビジュアライズすると、以下のツイートの添付動画のようになりました:

実際の正しい位置に対してそれらしく推定ができていそうです。

以下ではカルマンフィルタをマスターズ A 問題に応用するにあたって、状態空間モデルの作り方について記述していきます。
状態空間モデルに落とせたらあとは前節の方法で状態を推測すればいいだけです。

状態$x(t)$と状態遷移行列$F(t)$について

この問題では、ドローンの状態として x,y 方向の位置と速度があります。従って、状態は 4 次元のベクトル

$$x(t) = (p_x, p_y, v_x, v_y)^t$$

とするのが良さそうです。

また、入力やノイズがなかった場合に、 1 ターン後に状態がどう変化するかを状態遷移行列 F(t)に記述します。
速度の分だけ位置が変わるので、

$$F(t) = \begin{pmatrix} 1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix}$$

の 4 次元の正方行列となります。

観測値$y(t)$と観測行列$H(t)$について

観測できるのは壁からの距離なので、x 座標と y 座標を観測できると考えれば良さそうです。
従って、

$$y(t) = (p_x, p_y)^t$$

の 2 次元ベクトルにします。

また、ドローンの位置を観測することになるため、観測行列$H(t)$は、

$$H(t) = \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{pmatrix}$$

となります。

制御入力 u(t)と制御入力行列$G(t)$について

ドローンのコントロールのために、x 方向と y 方向に対して加速度を加えることができます。
従って、制御入力としては 2 次元のベクトル

$$u(t) = (a_x, a_y)^t$$

としておけば良さそうです。
(4 次元のベクトルをとって、最初の 2 つの項は常に 0 という整理にしても大丈夫です)

なお、加速しなかった場合(観測をした場合)には、

$$u(t) = (0, 0)^t$$

と取ることになります。

次に、制御入力が状態の発展に対してどのような影響を与えるかの制御入力行列$G(t)$について考えます。
速度が変わるのは良いとして、問題の設定として(速度の変更) -> (変更した速度で移動)となっているので、制御入力は位置にも影響があります。従って、

$$G(t) = \begin{pmatrix} 1 & 0 \\ 0 & 1 \\ 1 & 0 \\ 0 & 1 \end{pmatrix}$$

となります。

システムノイズの共分散行列$W(t)$について

状態の時間発展に対するノイズであるシステムノイズは、共分散行列$W(t)$のホワイトノイズになっています。
風の影響で標準偏差$\epsilon$だけ速度が毎ターン変化する効果をここに取り入れます。

この問題では、風の影響は x 方向と y 方向の速度に対してそれぞれ独立に作用するため、システムノイズ $w(t)$ は 2 次元のベクトルとなります：

$$w(t) = (w_x, w_y)^t$$

このノイズの共分散行列は：

$$W(t) = \begin{pmatrix} \epsilon^2 & 0 \\ 0 & \epsilon^2 \end{pmatrix}$$

そして、この 2 次元のノイズが 4 次元の状態に影響を与えるためのシステムノイズ入力行列 $B(t)$ は：

$$B(t) = \begin{pmatrix} 1 & 0 \\ 0 & 1 \\ 1 & 0 \\ 0 & 1 \end{pmatrix}$$

となります。制御入力と同様の議論で、風の影響は速度だけでなく位置にも影響があるため、このような形になります。

観測ノイズの共分散行列$V(t)$について

観測ノイズの共分散行列$V(t)$を考えます。
簡単のため、観測する方向は x または y 方向に観測をして、壁までの距離から位置を推定するものとします。

測定精度は壁までの距離に比例しているため、毎回観測ノイズの共分散行列は異なります。精度を壁までの距離$d$と入力から与えられる$\delta$を使って$d \delta$と表すことができるので、分散にはこれの二乗を与えれば良さそうです。

また、観測していない方向については、極めて分散が大きい観測(=精度の悪い観測)をしたということにして、共分散行列を作ることにします。この時の標準偏差を$\epsilon_{large}$とします。これは盤面よりも遥かに大きな値を取ると良いと思います。
同じ理屈で、観測をしなかった場合(=加速をした場合)には、めちゃくちゃ精度の悪い観測をしたということにして、共分散行列を作ることになります。

以上をまとめると、

• (x 方向を観測した時)

$$V(t) = \begin{pmatrix} (d \delta)^2 & 0 \\ 0 & \epsilon_{large}^2 \end{pmatrix}$$

• (y 方向を観測した時)

$$V(t) = \begin{pmatrix} \epsilon_{large}^2 & 0 \\ 0 & (d \delta)^2 \end{pmatrix}$$

• 観測しなかった場合 = 加速をした場合

$$V(t) = \begin{pmatrix} \epsilon_{large}^2 & 0 \\ 0 & \epsilon_{large}^2 \end{pmatrix}$$

と状況に応じて共分散行列を作り替えると良いでしょう。

モデリング完了

以上でモデリングが完了です!
ここまでくると、カルマンフィルタを使って、観測した位置から逐次的に状態およびそのエラーの共分散行列の推定をできるようになりました。

あとは選択した目的地に向けて、現在地と速度からいい感じに加速をしていけば良いことになります。
こちらのテクニックはeijirou さんの記事に詳しく書かれています。

最後に

学部が工学系でなかったので制御工学などは履修しておらず、初めて勉強した分野になりますが、なかなか面白かったです。
触ってみる前は難しそうだなあという漠然とした印象しかなかったのですが、実際にやってみると実装も簡単で使いやすく、状態の推定誤差と観測の誤差のバランスをうまく取る優れた方法であることが理解できました。

マスターズの問題はかなり変わり種の問題なので、同種の問題が AHC で今後出得るかというと微妙ですが、カルマンフィルタの気持ちはなんとなくわかったので、チャンスがあればぜひ使ってみたいと思っています。

(補足)C++によるカルマンフィルタライブラリの実装

実装したライブラリを参考として載せておきます。カルマンフィルタは有名な手法なので、AI がかなりいい感じに実装してくれます。

以下の 3 つを実装しました:

• 時間発展する状態を表すクラス KalmanState
• 観測値を表すクラス Observation
• カルマンフィルタを表すクラス KalmanFilter

次元は問題によって異なると思うので、template の機能を使って次元を可変にしています。

また、行列計算の際には eigen を使用しています(AtCoder 環境には入っている)。手元で動かす際にはご注意いただければと思います。

/**
 * @brief カルマンフィルタの状態を表すクラス
 *
 * @tparam StateSize 状態ベクトルのサイズ
 */
template <int StateSize>
struct KalmanState
{
    Eigen::Matrix<double, StateSize, 1> x;         // 状態ベクトル
    Eigen::Matrix<double, StateSize, StateSize> P; // 共分散行列

    KalmanState()
    {
        x.setZero();
        P.setIdentity();
    }

    KalmanState(const Eigen::Matrix<double, StateSize, 1> &state,
                const Eigen::Matrix<double, StateSize, StateSize> &covariance)
        : x(state), P(covariance) {}
};

/**
 * @brief 観測値を表すクラス
 *
 * @tparam ObservationSize 観測ベクトルのサイズ
 */
template <int ObservationSize>
struct Observation
{
    Eigen::Matrix<double, ObservationSize, 1> y; // 観測ベクトル

    Observation()
    {
        y.setZero();
    }

    explicit Observation(const Eigen::Matrix<double, ObservationSize, 1> &measurement)
        : y(measurement) {}
};

/**
 * @brief カルマンフィルタクラス
 *
 * 状態方程式: x(k+1) = A x(k) + B1 u(k) + B0 w(k)
 * 観測方程式: y(k) = C x(k) + v(k)
 *
 * @tparam StateSize 状態ベクトルのサイズ
 * @tparam InputSize 制御入力ベクトルのサイズ
 * @tparam ObservationSize 観測ベクトルのサイズ
 */
template <int StateSize, int InputSize, int ObservationSize>
class KalmanFilter
{
public:
    using StateVector = Eigen::Matrix<double, StateSize, 1>;
    using StateMatrix = Eigen::Matrix<double, StateSize, StateSize>;
    using InputVector = Eigen::Matrix<double, InputSize, 1>;
    using InputMatrix = Eigen::Matrix<double, StateSize, InputSize>;
    using ObservationVector = Eigen::Matrix<double, ObservationSize, 1>;
    using ObservationMatrix = Eigen::Matrix<double, ObservationSize, StateSize>;
    using KalmanGain = Eigen::Matrix<double, StateSize, ObservationSize>;

    /**
     * @brief コンストラクタ
     */
    KalmanFilter()
    {
        A.setIdentity();
        B1.setZero();
        B0.setZero();
        C.setZero();
    }

    /**
     * @brief パラメータ付きコンストラクタ
     *
     * @param state_transition 状態遷移行列 A
     * @param control_input 制御入力行列 B1
     * @param process_noise プロセスノイズ入力行列 B0
     * @param observation 観測行列 C
     */
    KalmanFilter(
        const StateMatrix &state_transition,
        const InputMatrix &control_input,
        const StateMatrix &process_noise,
        const ObservationMatrix &observation)
        : A(state_transition), B1(control_input), B0(process_noise),
          C(observation)
    {
    }

    /**
     * @brief システム行列の設定
     *
     * @param state_transition 状態遷移行列 A
     * @param control_input 制御入力行列 B1
     * @param process_noise プロセスノイズ入力行列 B0
     * @param observation 観測行列 C
     */
    void setSystemMatrices(
        const StateMatrix &state_transition,
        const InputMatrix &control_input,
        const StateMatrix &process_noise,
        const ObservationMatrix &observation)
    {
        A = state_transition;
        B1 = control_input;
        B0 = process_noise;
        C = observation;
    }

    /**
     * @brief 初期状態の設定
     *
     * @param initial_state 初期状態ベクトル
     * @param initial_covariance 初期共分散行列
     * @return KalmanState<StateSize> 初期化された状態
     */
    KalmanState<StateSize> initialize(
        const StateVector &initial_state,
        const StateMatrix &initial_covariance)
    {
        return KalmanState<StateSize>(initial_state, initial_covariance);
    }

    /**
     * @brief 予測ステップ（時間更新）
     *
     * @param current_state 現在の状態
     * @param control_input 制御入力 u(k)
     * @param process_noise_cov プロセスノイズ共分散行列
     * @return KalmanState<StateSize> 予測された次の状態
     */
    KalmanState<StateSize> predict(
        const KalmanState<StateSize> &current_state,
        const InputVector &control_input,
        const StateMatrix &process_noise_cov)
    {
        // 状態の予測
        StateVector predicted_x = A * current_state.x + B1 * control_input;

        // 共分散行列の予測
        StateMatrix predicted_P = A * current_state.P * A.transpose() + B0 * process_noise_cov * B0.transpose();

        return KalmanState<StateSize>(predicted_x, predicted_P);
    }

    /**
     * @brief 更新ステップ（観測更新）
     *
     * @param predicted_state 予測された状態
     * @param observation 観測値
     * @param observation_noise_cov 観測ノイズ共分散行列
     * @return KalmanState<StateSize> 更新された状態
     */
    KalmanState<StateSize> update(
        const KalmanState<StateSize> &predicted_state,
        const Observation<ObservationSize> &observation,
        const Eigen::Matrix<double, ObservationSize, ObservationSize> &observation_noise_cov)
    {
        // イノベーション（予測誤差）の計算
        ObservationVector innovation = observation.y - C * predicted_state.x;

        // イノベーション共分散行列
        Eigen::Matrix<double, ObservationSize, ObservationSize> S =
            C * predicted_state.P * C.transpose() + observation_noise_cov;

        // カルマンゲインの計算
        KalmanGain K = predicted_state.P * C.transpose() * S.inverse();

        // 状態の更新
        StateVector updated_x = predicted_state.x + K * innovation;

        // 共分散行列の更新
        StateMatrix updated_P = (StateMatrix::Identity() - K * C) * predicted_state.P;

        return KalmanState<StateSize>(updated_x, updated_P);
    }

    /**
     * @brief 予測と更新を一度に行う
     *
     * @param current_state 現在の状態
     * @param control_input 制御入力
     * @param observation 観測値
     * @param process_noise_cov プロセスノイズ共分散行列
     * @param observation_noise_cov 観測ノイズ共分散行列
     * @return KalmanState<StateSize> 更新された状態
     */
    KalmanState<StateSize> step(
        const KalmanState<StateSize> &current_state,
        const InputVector &control_input,
        const Observation<ObservationSize> &observation,
        const StateMatrix &process_noise_cov,
        const Eigen::Matrix<double, ObservationSize, ObservationSize> &observation_noise_cov)
    {
        // 予測ステップ
        KalmanState<StateSize> predicted_state = predict(current_state, control_input, process_noise_cov);

        // 更新ステップ
        return update(predicted_state, observation, observation_noise_cov);
    }

private:
    StateMatrix A;       // 状態遷移行列
    InputMatrix B1;      // 制御入力行列
    StateMatrix B0;      // プロセスノイズ入力行列
    ObservationMatrix C; // 観測行列
};