p5jsではじめるカルマンフィルタ実装

# はじめに
皆さん、はじめまして！
今回はp5jsを用いて、カルマンフィルタを実装してみようと思います。
私自身、特にプログラミングに詳しいわけではないので、対象は以下となります

:::note info
対象
  - プログラミングに興味がある人
  - 環境構築の時点で、やる気ゲージが0になってしまう人
  - せっかくプログラミングをするなら、ビジュアルにもこだわりたい人
:::

# p5jsとは
[p5jsの公式サイト](https://p5js.org/)には、次のような説明がされています。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3621325/f5adf0ad-600d-8ec9-b913-a79baa1c9864.png)

>p5jsとは、クリエイティブなコーディングのためのJavaScriptライブラリであり、アーティスト、デザイナー、教育者、初心者など誰にとってもコーディングにアクセスしやすく、受け入れやすいものにすることに重点を置いています。
(公式サイト原文翻訳)

元々、processingというアーティスティックな表現を容易に可能な言語があり、p5jsでは、ブラウザ上で気軽に試すことが可能です。
環境構築しないので、やる気ゲージが失われる前に、取り組めますね！

processingで著名な方に、Daniel Shiffmanという方がおられます。
彼は、processingの開発のリードであり、ニューヨーク大学Tisch School of the Artsスクールの准教授でもあるそうです。
Daniel Shiffmanさんは、[Coding Train](https://www.youtube.com/c/TheCodingTrain/videos)というYoutubeチャンネルでprocessingやp5jsを用いて、実際に実装しながらプログラミングの解説をしています。彼のコーディングスタイルは、丁寧かつ陽気なので楽しみながら、プログラミング(と英語)を学ぶことができます。

>スネークゲーム
<iframe width="560" height="315" src="https://www.youtube.com/embed/AaGK-fj-BAM?si=_nOOS6_x1i5lc79W" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

>アスキーアート
<iframe width="560" height="315" src="https://www.youtube.com/embed/55iwMYv8tGI?si=7YFRPNz1h8s4dZae" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>


サムネイルから楽しそうな様子が伝わってきます。陽気ですね。
彼の書籍である[「Nature of Code」](https://natureofcode.com/)は、無料で公開されており、またCoding Trainチャンネルのコミュニティもあるので、興味のある方は是非覗いてみてはいかがでしょうか？

# カルマンフィルターとは
Daniel Shiffmanさんの宣伝ばかりしていますが、ここからは、カルマンフィルターをp5js上で実装していこうと思います。
### カルマンフィルターざっくり説明
カルマンフィルタとは、観測データによって、対象とする物体や現象の真の状態を推定するアルゴリズム(方法論？)です。

アルゴリズムの導出は、(私が説明するのが)難しいので他サイトに任せます。

https://logics-of-blue.com/kalman-filter-concept/

https://disassemble-channel.com/the-theory-of-kalman-filter/

めちゃざっくり流れだけ説明すると、状態変数というものを定義し、時間発展方程式による予測と観測データによる修正(フィルタ)を繰り返し、状態変数を真の状態に近づけていきます。(と理解しています。)

カルマンフィルタは、目的に応じて、アンサンブルカルマンフィルタ、粒子フィルタなど派生、進歩しており、その活用分野は自動運転システム、天気予報、宇宙ロケットなど、多岐に渡ります。
なんだかカルマンフィルタがかっこよく見えてきました！

### 今回、カルマンフィルターで表現するもの
今回は、マウスポインタの位置を追跡する点、つまりマウストラッキングを実装してみましょう。位置や速度を推定するのは、自動運転やロケットの姿勢制御にも使われているので、マウストラッキングはその第一歩という感じでワクワクしますね！

### 数式
書籍やWebサイトを参考に、自分なりに立式してみました。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3621325/57ca8912-6daf-d178-e983-cae08fbbd6e6.png)

ここで、自分で考えたのは、状態変数、観測データの定義とそれらを紐づける観測演算子、状態の時間発展方程式です。
それ以外は、カルマンフィルタのアルゴリズムに基づく式になります。

今回は、マウス位置を推定したいのにも関わらず、マウス位置を観測データとしているので、とても簡単な状態推定ができるでしょう。(いわば、真の状態が観測値として得られている状態)

また状態変数に加速度まで追加しましたが、位置、速度だけでもマウス位置の推定は可能です。

本来のカルマンフィルタでは、状態変数の次元が莫大であったり、状態の時間発展が非線形である事象を対象としているために、様々な工夫が施されています。

一例に、スーパーコンピューター富岳を用いて、局所アンサンブル変換カルマンフィルタにより、より高精度に天気予報を行った記事を紹介します。
(大分端折っているので,ぜひリンク先をご覧ください！)

https://www.r-ccs.riken.jp/highlights/pickup4/

# 実装
立式した数式をコードにしていきます。
ここでは、すべてのコードについてコメントできないので抜粋して説明します。
文法やライブラリに関しては、[公式リファレンス](https://p5js.org/reference/)を参照ください
### 実装ポイント
#### プログラムの流れ
プログラムは、スクリーンショットのように、setup関数とdraw関数で構成されます。
setup関数は、プログラム実行時に1度だけ実行、draw関数は、フレーム毎に実行されます。

ここでは、観測データ(マウス位置)についての処理をObservedクラス、状態変数(推定点)についての処理をEstimaterクラスで定義しています。
それぞれ、毎フレーム(draw関数内)で、以下の処理を行います。
- Observed
    - update : マウス位置を更新
    - addnoise : 観測データにノイズを加える
    - show : マウス位置を描画
- Estimater
    - predict : 推定点の現在位置、速度、加速度から次フレームの位置を予測
    - KalmanFilter : 観測データ(マウス位置)により、位置を修正
    - show : 推定点の描画
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3621325/f0e3f5a4-28b8-9014-1487-0a8ab6b55c44.png)

#### マウス位置と推定点の表現
マウス位置は、このようにクロスマークを描画する関数を定義し、位置を引数にし、毎フレーム描画することで、表現しています。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3621325/8170d4f5-068d-2fa8-3ce5-84f3f259e57e.png)
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3621325/63017524-993e-b2dd-3928-584660e1748e.png)

推定点は、シンプルに白点として表現しています。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3621325/75649a8e-def8-fee8-da33-e3e4577f7ed0.png)

また移動経路が分かりやすいように、残像を見えるようにしています。

### 実装コードの公開

実装したコードのほとんどにコメントできていないので、こちらで実際のコードと動くものを公開します。

https://editor.p5js.org/NOWIII/full/fmLQarX_9

左上のアイコンからコードが参照でき、プログラムを変えて再実行できます。いろいろいじってみてください！

ピンク色のクロスマークがマウスの位置で、白い点が少し遅れながらついてくると思います。
かわいいですね！
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3621325/2d117850-5c64-d2f9-03f6-5ad1c213bee9.png)

# 実験
カルマンフィルタの特徴を知り、理解を深めるために簡単な実験をしてみましょう。
### 実験1 : 観測演算子を変更
実験1では、観測データとの紐づき方を変えてみます。
上記のカルマンフィルタでは、観測データと状態変数を紐づける観測演算子Hを以下のように定義してみました。1,0をうまく配置し、定義することで、観測データと状態変数の位置を紐づけています。
```math
H = \begin{bmatrix}1 & 0 & 0 & 0 & 0& 0\\0 & 1 & 0 & 0 & 0& 0 \end{bmatrix}
```
こちらを、下記のように変更してみます。2列2行目の1が0になりました。観測データと推定点の位置のx座標のみしか紐づけられていないことを意味します。
```math
H = \begin{bmatrix}1 & 0 & 0 & 0 & 0& 0\\0 & 0 & 0 & 0 & 0& 0 \end{bmatrix}
```

この状態で、マウストラッキングを行うとどうなるでしょうか？
以下のリンクから体験できます。

https://editor.p5js.org/NOWIII/full/qSP0VT4rN

スクリーンショットから分かる通り、x方向には追従しているものの、マウス位置のy座標が観測データとして渡されていないので、y方向には追従されていないです。
実際のカルマンフィルタでは、観測データが何かを把握し、そこから現実的に推定できる状態変数を整理し、観測演算子を定義すべきでしょう。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3621325/5d721b5e-b495-9e51-dcb7-3c7209ef32dc.png)

### 実験2 : フィルタ頻度を変更

実験2では、フィルタリングを行う頻度を変えてみます。先ほどまでは、予測とフィルタを交互に行っていました。しかし、現実の事象では、予測の時間増分と観測データの得られる頻度が同等であるとは限りません。ここでは、10回の予測をした後、1回の観測データが得られるとします。

実験2のコードがこちらです。

https://editor.p5js.org/NOWIII/full/cp8ZLp9ks

スクリーンショットでは、少し分かりにくいですが、マウスがカーブしていても、その時点で観測データが渡されていないので、推定点はまっすぐ進んでいます。そして、10回予測した後、最新の観測データにより、位置が修正されています。マウスを早く動かせば動かすほど、推定点はその時点の加速度から次点のマウス位置を予想し、遠くに飛んでいきます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3621325/1f46e1dc-047d-cf7c-4a93-2b7fb5a0d591.png)

## 最後に
この記事では、気軽にプログラミングができるp5jsの紹介、様々な分野で活用されているカルマンフィルタの紹介と実装、数式やコードをいじる実験をしてみました。

初投稿記事ということもあり、不十分な点が多くあると思いますが、これを機にp5js等でのプログラミングをはじめてみたり、カルマンフィルタに興味を持っていただけたりすると嬉しいです。

不明点、間違っている点あれば、コメントお願いします！

ここまでお読みいただきありがとうございました！🎉