粗大メモ置き場

個人用，たまーに来訪者を意識する雑記メモ

足立先生の「カルマンフィルタの基礎」の誤植について～MATLABを用いた代数リカッチ方程式の求解～

カルマンフィルタ matlab

誤植はどの本にもあることです。 なお，手元にあるのは第４版です。

カルマンフィルタの基礎

カルマンフィルタの基礎

線形カルマンフィルタの終端値とリカッチ方程式

線形カルマンフィルタの事前推定の共分散をPとします。 十分時間が経って定常値となる時のカルマンゲインは

K = B^\top A^{-1} = P C^\top (C PC^\top + R)^{-1}

この時，終端値において次の式が成り立ちます。

P = A(I-KC)PA^\top ＋ Q

もっと長く書くとこう。

P = A(I-P C^\top (C PC^\top + R)C)^{-1} PA^\top ＋ Q

展開すると

APA^\top -A - AP C^\top (C PC^\top + R)^{-1}CPA^\top ＋ Q=0

となり，これは代数リッカチ方程式（DARE）となります。

MATLABで解けます。

jp.mathworks.com

MATLABでリカッチ方程式を解く解法

Referenceによると，

f:id:ossyaritoori:20190522013718p:plain

だそうです。

従って，

[P,L,G]=dare(A',C',Q,R)

と実行することで解を得られます。

誤植箇所

p149 演習６-２の解答

解答ではPの最終値が1+√３としていますが，

[P,L,G]=dare(1',1',4,1)

を計算すると，2(1+√2)=4.8前後が正解の値になるはずです。 なお，スカラなので手計算でリカッチ方程式を解いても同様の値になります。

ドヤ顔で書いてますが間違ってたらすみません。

jupyter notebookのセル出力を横にしたら結構良かった

jupyter

2019年5月現在，jupyter 4.4.0 にて動作確認。

notebookのセル出力を横にしたい

notebookはhtmlで編集できるらしいので以下のようにセル出力を横にできます。 幅も広くとれて使い勝手良さそう。

f:id:ossyaritoori:20190505230104p:plain
図がでかいとちょっと無駄感もあるかもしれない…

設定方法

ここの議論を参考にしました。

以下のコードをセルで実行するだけ。

%%html
<style>
#notebook-container {
    width: 90%;
    background-color: #EEE
}

.code_cell {
   flex-direction: row !important;
}

.code_cell .output_wrapper {
    width: 50%;
    background-color: #FFF
}

.code_cell .input {
    width: 50%;
    background-color: #FFF
}
</style>

用途によって左右の比を変えてもいいかと。

なお，Githubのページでは普段どおりに写ります。

f:id:ossyaritoori:20190506205801p:plain
Githubでは普通にうつる

カルマンフィルタのゲイン導出メモと共分散行列の遷移

注：これは個人用メモです。

はじめに

もう一度いいますがこれは個人用メモです。

わかりやすい説明などは以下のブログ様などを参考にしたほうが良いです。

hamachannel.hatenablog.com

差別点は

  • 多変量の場合を考慮している点
  • 変分ではなく平方完成で導出を行っている点

でしょうか。自分用に覚書がほしかった箇所をメモっています。

本記事の流れ

以下の流れで行きます。

  • オブザーバ（濾波型）の推定式からk+1の時の誤差共分散を導出
  • 誤差共分散行列を平方完成して最適ゲイン＝カルマンゲインを導出
  • その時の共分散の更新則を導出

推定に依る誤差共分散の式

  • 予測ステップは以下の通りとします。uは入力，vはシステムノイズです。

\hat{x}_{k|k-1} = A \hat{x}_{k-1} + B u_{k-1}+ v_{k-1}

  • 修正ステップは以下の様に書けます。ここでFはフィードバックゲインです。

\hat{x}_{k} = \hat{x}_{k|k-1} + F(y_k-\hat{y}_k)


また，観測方程式から \hat{y}_{k} = C\hat{x}_{k|k-1} ですのでこれを代入して

\hat{x}_{k} = (I-FC) \hat{x}_{k|k-1} + F(y_k)


したがって，誤差共分散は各々の要素が独立だとして以下のように書けます。

V(\hat{x}_{k}) = V((I-FC)\hat{x}_{k|k-1}) + V(Fy_{k})


そろそろ面倒なのでここから\hat{x}_{k|k-1}=xとして書いてしまいます。

  • V(AX) = AV(X)A^\top
  • V(y) = W の関係を用いて書き下すと，

V(\hat{x}_{k}) = (I-FC)V(x)(I-FC)^\top + FV(y)F^\top \\ = FCV(x)C^\top F^\top-FCV(x)-V(x)C^\top F^\top +V(x) +F W F^\top

となります。

誤差共分散を最小化するゲインの導出

後は，変分法でもいいのですが今回は平方完成を用います。

簡単のため， C V(x)C^\top +W=A,そして CV(x)=Bと置きます。

FCV(x)C^\top F^\top-FCV(x)-V(x)C^\top F^\top +V(x) +F W F^\top \\ = FAF^\top - FB - B^\top F^\top + V(x)


先ほどの式をFに関する2次系にすると

(F-B^\top A^{-1})A(F-B^\top A^{-1})^\top + V(x) - B^\top A^{-1}B

となります。


したがってこれを最小化するゲインFは自明に

F = B^\top A^{-1} = P C^\top (C PC^\top + W)

となるわけです。（公式と見比べるためにP=V(x)としました。）


またこの時，最小値は

V(x) - B^\top A^{-1}B = P - FCP = (I-FC)P

となり，公式と同様の値が導出できます。

最適ゲインでないゲインを用いた場合の共分散

最適でないゲインF'を用いた際の誤差共分散は

(I-F’C)P

という簡単な式ではなく，

(I-F'C)P(I-F'C)^\top + F'WF'^\top

をきちんと計算しなくてはいけません。

カルマンフィルタの公式だけを眺めていて一度間違った式を使っていたことがあります。

疑問

ゲインFに何らかの制約があった時に，以下の式

(F-B^\top A^{-1})A(F-B^\top A^{-1})^\top + V(x) - B^\top A^{-1}B

を最小化（ノルムを？）するにはどうすればいいんでしょう？高校数学的な解法は線形代数の場合はどの様に計算するんでしょうね？？？


追記：LMIの目的関数にしてYalmipで解いてみましたがソルバがないと言われてしまいました。目的関数は非線形になるんですが，凸っぽいような…？

参考文献

今回の導出は足立先生の本にのっています。ベイズ統計などは確率ロボティクスなどを読むとよいです。 ベイズの定理に従うと勝手に最尤値を計算してしまうので今回のような導出にはなりませんが。

カルマンフィルタの基礎

カルマンフィルタの基礎

確率ロボティクス (ROBOT books)

確率ロボティクス (ROBOT books)

線形代数は以下の本がいいそうで…

統計のための行列代数 上

統計のための行列代数 上

統計のための行列代数 下

統計のための行列代数 下

Raspberry Pi Mouseでサーボモータ（SG90）を動かす

Raspberry Pi raspimouse python

Raspberry Pi Mouse上でのGPIOの配線

SG90はラズパイ上のGPIOポートを用いて制御しますが，ラズパイマウスではそのポートは覆い隠されてしまいます。

しかし，外のデバイスを繋げないかというとそうでもなく，以下の写真の箇所に接続可能なポイントがあります。

f:id:ossyaritoori:20190422195726p:plain
本体右手側にピンがあります。

ここに書かれている英字は以下のGPIO番号（ピン番号ではない）と対応しています。

  • SDA → GPIO2
  • SCL → GPIO3
  • TXD → GPIO14
  • RXD → GPIO15

詳しいピンの位置等は以下の図を参照してください。 https://hackster.imgix.net/uploads/attachments/218603/6sQiFTKXhZptFiGnPlsc.png

従って，先程の写真のようにオスのピンヘッダをはんだ付けすれば外部出力用として使えます。

ラズベリーパイでSG90を制御する

SG90とは安価に手に入るお手軽なサーボモータです。 可動域は180degでトルクは1.8kgfcm出ます。これは見かけよりはパワーが有ると思って良いです。

デジタル・マイクロサーボ SG90 （5個）

デジタル・マイクロサーボ SG90 （5個）

VCCとGNDに加えて制御用の信号が1つの計3つの信号線が必要です。信号はduty比を変えた矩形波を入力しますが詳しい説明に関してはこちらのサイトがわかりやすいと思います。

私はSDA，すなわちGPIO ポート2を信号線用に使うことにしました。

  • 以下補足情報

本来求められるPWM信号は4.8VくらいでGPIOは3.3Vしか出ないのですが，一応直挿しでも動きはします。ただ，時々ちょっと振動的な挙動を示すので精度が必要ならちゃんとやるべきかも（サンプル数１）

SG90用コントローラ クラス

初期化などの処理をまとめたかったのでクラスで実装しました。 最新版はここで管理しますが，現在は以下のような感じに書いています。

大まかな挙動は

  • ポート番号を渡して初期化
  • 角度をdegreeで渡して制御
  • stop()関数でサーボ状態から抜ける

となっています。

import RPi.GPIO as GPIO

class SG90servo():
    def __init__(self,motorport=2):
        self.port = motorport
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.port, GPIO.OUT)
        self.servo = GPIO.PWM(self.port, 50)

        self.servo.start(0.0) # set duty cicle to 0-100
        self.minlim = -90
        self.maxlim = 90

    def servo_deg(self,deg):

        if deg > self.maxlim:
            print("Exceed max degree!")
            return
        elif deg < self.minlim:
            print("Exceed min degree!")
            return
        
        cycle = 7.25+4.75*deg/90.0
        self.servo.ChangeDutyCycle(cycle)

    def stop(self):
        self.servo.start(0.0)

宣伝枠

ラズパイマウス，ちょっと高いですが手軽に勉強できるので教育的にはいいのかなと思います。

Raspberry Pi Mouse V2 フルキット

Raspberry Pi Mouse V2 フルキット

Raspberry Piで学ぶ ROSロボット入門

Raspberry Piで学ぶ ROSロボット入門

MarkdownとVScodeで爆速PDFレポート作成（Pandoc,Latex経由なし）

Markdown Vscode

目的

以下の縛りのあるレポートをMarkdownを使って書きます。

  • 体裁指定のない小レポート
  • タイトル・所属を書けばあとは自由

PandocやLatexを経由しないため手間を最小化できます。

必要なもの

VScode周りは以下の記事などを参照に ossyaritoori.hatenablog.com

設定

余分なヘッダーの削除

デフォルトでは謎のHeaderがついています。邪魔です。

qiita.com

上の記事に倣って，

  • 「ファイル」→「基本設定」→「設定」を開く
  • markdown-pdf.headerTemplateで検索
  • setting.jsonに"markdown-pdf.headerTemplate": "<span class='title' style='display: none;'></span>" を追記

を行います。

テンプレート

タイトルは#で，以下セクションは##で書きます。 所属のところだけhtmlで右揃えにすればそれっぽいですね。

# タイトル
<div style="text-align: right;">
所属とか日付とか
</div>

## 1. Sectionは##で

ほげほげ

## 2. Sectio番号は手打ち
ほむほむ

### 2-1. Subsectionは`###`で
ふがふが
### 2-2. 余談だが
- Ctrl+Alt+P をPDF変換のショートカットに登録したがいい感じ
  • 出力はこんなの

f:id:ossyaritoori:20190421234819p:plain
外観の例

Section Numberingの自動化

章の番号を自動で割り振る機能がある。 右クリックから呼び出すメニューでToggle可能。

marketplace.visualstudio.com

もっと頑張りたい人に

Pandocを使いましょう。というか直接Latex書いてもいいかと。

qiita.com

Raspberry Pi Mouseのカメラマウントファイルを分割して3Dプリントした話

Raspberry Pi

あらまし

今日始めて近くの3Dプリントサービスを使ってきました。

プリントしたのはラズパイマウスのcameraマウンタです。 DMMmakerさんで高精度にプリントされたものが2700円で売ってます。

https://products.rt-net.jp/micromouse/wp-content/uploads/sites/3/2016/01/main_l.jpg

詳細はこちら。

これを自前で印刷してみようと，近所の工房に来て印刷しました。 お値段は810円でした。（失敗して二度印刷しましたがまけてもらいました^^）

所要時間：28分 \ 積層厚さ：2.8mm \ サポート材：あり（なしで失敗）\ 費用：810円（税込）

3Dプリント時の注意点

一応今回のプリントする時の注意みたいなのをメモします。 超基礎事項ですので経験者は飛ばしてみてください。

  • 向きに気をつける必要がある
  • サポート材は基本ありでやる

向きとは例えばネジ穴などの箇所は穴の中心線に対して垂直にしたほうがいいとかそういうことみたいです。 成形物は鉱物のように積層方向に割れやすいので穴に対して縦に裂けるのを避けるという意図があるそうな。

また，置く向きによって支えが必要になるのでそのことも考えて設計・配置しなければいけないみたいですね。

stlファイルの分割

該当のstlファイルはただで配っているのでそのまま落としてくればいいんですが，部品がくっついていてどうも向きなどの調整が難しいので分離してしまおうと思いました。

工房のPCにはMeshmixerというソフトが入っていたのでそれをつかってやったことをメモします。 基本的には下記のサイトに書いてあることに倣えばよさそうです。

figrobo.blog.fc2.com

  1. 選択ツールでつなぎ目を選択
  2. 編集ツールで削除
  3. 余計な箇所も削れるので解析ツールで解析して修復
  4. 選択ツールで部品を選択，エクスポートを行う
  5. 選択後の編集ツールの「分離」という操作でもオブジェクトを分解できるみたい

とりあえず何もわかってませんが分離自体はできました。

分離したファイル

分離したファイルをここに配布します。

drive.google.com

プリントしたところ，cameraに接続する側の正方形の部品が膨らみ気味ではまらなかったので精度を上げるか，プリント後に削るかしたほうが良いでしょう。

プリントした結果

  • 一回目（サポート材無し）

    f:id:ossyaritoori:20190420201355p:plain
    四足で浮いている部品がうまく印刷できなかった

  • 二回目（サポート材あり）

    f:id:ossyaritoori:20190420201521p:plain
    左：サポート材なし，右：サポート材あり

サポート材を入れないと浮いている箇所は当然失敗します。そらそうや。

後始末

プリント後のサイズが若干合わなかったのでちょっと削ります。ヤスリでやったけどちょっと不格好かも。

所感・まとめ

  • 自前で3Dプリントやるのは結構手間がかかるのとそんなに精度が出ない
  • お金と時間に余裕があるならDMM等に頼んでも良さそう

MATLABでnumpyで保存したcsvファイルを開く

MATLAB python

結論

書きはじめの時は，numpyで保存したcsvがmatlabに変な感じで認識されるのを不満に思っていたのですが，途中から自分の過ちに気づきました．

要点は以下の２つです．

  • csvread()は区切りがカンマでなくては行けない
  • そうでない場合はreadtable()を使う

はい．帰ってよし．

numpy arrayの保存

自分はもっぱらsavetxtを用います．

numpy.savetxt("K.csv", K, delimiter =',',fmt="%0.14f")

loadする時，そのままnumpy arrayとしてloadでき，他のソフトからも開きやすいからです．（Excelはダメ）

こんなデータになる

糞長くなるので人間にわかりやすいデータではないですが，一応こんな感じにsaveされます． 嘘です．delimiter =','という引数を加えないと区切りがスペースになってしまいます． 其の結果が以下の通りです．

1.554141360341085672e+09 8.172370370370369983e+02 4.427456790123457040e+02 -1.855009145168301770e-03 -4.460917310385187178e-03
1.554141360408607960e+09 8.172024691358025166e+02 4.427777777777777715e+02 -1.670469061790032239e-03 -4.653048558273605202e-03
1.554141360473935604e+09 8.172383292383292428e+02 4.427518427518427302e+02 -1.816789199264026364e-03 -4.465982169869531920e-03
1.554141360534764767e+09 8.171901234567901611e+02 4.427753086419753004e+02 -1.689939994465252445e-03 -4.698456650358831185e-03

MATLABでcsvを開く

最も直感的なのはドラッグアンドドロップしてuiopenとして開くことです． 直感的だけど多くのcsvを読む用途には適しません．

csvread：’，’をdelimiterにしないとおかしくなる．

一応，csvread(filename)とする手法がありますが，先ほどのcsvを読むと4行5列のarrayのはずなのに4行1列として表示されます．

ans =

   1.0e+09 *

    1.5541
    0.0000
    0.0000
   -0.0000

カンマで区切ってないからですね．

readtable：有能

readtableなら区切り文字がほかの文字でもうまく検出することができます． 一方で返り値はtableなのでtable2array()関数で変換する必要があります．

table2array(readtable(flename))

という感じです．

Reference

みんなもちゃんとリファレンス読みましょう．

How can I import data from .csv file with numeric values and texts (with column headers) into MATLAB Workspace? - MATLAB Answers - MATLAB Central

テキスト ファイルのインポート - MATLAB & Simulink - MathWorks 日本