カルマンフィルタのゲイン導出メモと共分散行列の遷移
注:これは個人用メモです。
はじめに
もう一度いいますがこれは個人用メモです。
わかりやすい説明などは以下のブログ様などを参考にしたほうが良いです。
差別点は
- 多変量の場合を考慮している点
- 変分ではなく平方完成で導出を行っている点
でしょうか。自分用に覚書がほしかった箇所をメモっています。
本記事の流れ
以下の流れで行きます。
- オブザーバ(濾波型)の推定式からk+1の時の誤差共分散を導出
- 誤差共分散行列を平方完成して最適ゲイン=カルマンゲインを導出
- その時の共分散の更新則を導出
推定に依る誤差共分散の式
- 予測ステップは以下の通りとします。uは入力,vはシステムノイズです。
- 修正ステップは以下の様に書けます。ここでFはフィードバックゲインです。
また,観測方程式からですのでこれを代入して
したがって,誤差共分散は各々の要素が独立だとして以下のように書けます。
そろそろ面倒なのでここからとして書いてしまいます。
- の関係を用いて書き下すと,
となります。
誤差共分散を最小化するゲインの導出
後は,変分法でもいいのですが今回は平方完成を用います。
簡単のため,,そしてと置きます。
先ほどの式をFに関する2次系にすると
となります。
したがってこれを最小化するゲインFは自明に
となるわけです。(公式と見比べるためにとしました。)
またこの時,最小値は
となり,公式と同様の値が導出できます。
最適ゲインでないゲインを用いた場合の共分散
最適でないゲインを用いた際の誤差共分散は
という簡単な式ではなく,
をきちんと計算しなくてはいけません。
カルマンフィルタの公式だけを眺めていて一度間違った式を使っていたことがあります。
疑問
ゲインFに何らかの制約があった時に,以下の式
を最小化(ノルムを?)するにはどうすればいいんでしょう?高校数学的な解法は線形代数の場合はどの様に計算するんでしょうね???
追記:LMIの目的関数にしてYalmipで解いてみましたがソルバがないと言われてしまいました。目的関数は非線形になるんですが,凸っぽいような…?
参考文献
今回の導出は足立先生の本にのっています。ベイズ統計などは確率ロボティクスなどを読むとよいです。 ベイズの定理に従うと勝手に最尤値を計算してしまうので今回のような導出にはなりませんが。
- 作者: 足立修一,丸田一郎
- 出版社/メーカー: 東京電機大学出版局
- 発売日: 2012/10/10
- メディア: 単行本(ソフトカバー)
- 購入: 3人 クリック: 3回
- この商品を含むブログ (5件) を見る
- 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox,上田隆一
- 出版社/メーカー: 毎日コミュニケーションズ
- 発売日: 2007/10/25
- メディア: 単行本(ソフトカバー)
- 購入: 1人 クリック: 27回
- この商品を含むブログ (6件) を見る
線形代数は以下の本がいいそうで…
- 作者: D. A.ハーヴィル
- 出版社/メーカー: 丸善出版
- 発売日: 2012/04/05
- メディア: 単行本(ソフトカバー)
- 購入: 3人 クリック: 2回
- この商品を含むブログを見る
- 作者: D. A.ハーヴィル
- 出版社/メーカー: 丸善出版
- 発売日: 2012/04/05
- メディア: 単行本(ソフトカバー)
- 購入: 2人 クリック: 3回
- この商品を含むブログを見る
Raspberry Pi Mouseでサーボモータ(SG90)を動かす
Raspberry Pi Mouse上でのGPIOの配線
SG90はラズパイ上のGPIOポートを用いて制御しますが,ラズパイマウスではそのポートは覆い隠されてしまいます。
しかし,外のデバイスを繋げないかというとそうでもなく,以下の写真の箇所に接続可能なポイントがあります。
ここに書かれている英字は以下のGPIO番号(ピン番号ではない)と対応しています。
- SDA → GPIO2
- SCL → GPIO3
- TXD → GPIO14
- RXD → GPIO15
従って,先程の写真のようにオスのピンヘッダをはんだ付けすれば外部出力用として使えます。
ラズベリーパイでSG90を制御する
SG90とは安価に手に入るお手軽なサーボモータです。 可動域は180degでトルクは1.8kgfcm出ます。これは見かけよりはパワーが有ると思って良いです。
- 出版社/メーカー: 梅本合同会社
- メディア: おもちゃ&ホビー
- この商品を含むブログを見る
VCCとGNDに加えて制御用の信号が1つの計3つの信号線が必要です。信号はduty比を変えた矩形波を入力しますが詳しい説明に関してはこちらのサイトがわかりやすいと思います。
私はSDA,すなわちGPIO ポート2を信号線用に使うことにしました。
- 以下補足情報
本来求められるPWM信号は4.8VくらいでGPIOは3.3Vしか出ないのですが,一応直挿しでも動きはします。ただ,時々ちょっと振動的な挙動を示すので精度が必要ならちゃんとやるべきかも(サンプル数1)
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)
宣伝枠
ラズパイマウス,ちょっと高いですが手軽に勉強できるので教育的にはいいのかなと思います。
- 出版社/メーカー: 株式会社アールティ
- メディア:
- この商品を含むブログを見る
- 作者: 上田隆一
- 出版社/メーカー: 日経BP社
- 発売日: 2017/03/30
- メディア: 単行本
- この商品を含むブログ (1件) を見る
MarkdownとVScodeで爆速PDFレポート作成(Pandoc,Latex経由なし)
目的
以下の縛りのあるレポートをMarkdownを使って書きます。
- 体裁指定のない小レポート
- タイトル・所属を書けばあとは自由
PandocやLatexを経由しないため手間を最小化できます。
必要なもの
VScode周りは以下の記事などを参照に ossyaritoori.hatenablog.com
設定
余分なヘッダーの削除
デフォルトでは謎のHeaderがついています。邪魔です。
上の記事に倣って,
- 「ファイル」→「基本設定」→「設定」を開く
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変換のショートカットに登録したがいい感じ
- 出力はこんなの
Section Numberingの自動化
章の番号を自動で割り振る機能がある。 右クリックから呼び出すメニューでToggle可能。
もっと頑張りたい人に
Pandocを使いましょう。というか直接Latex書いてもいいかと。
Raspberry Pi Mouseのカメラマウントファイルを分割して3Dプリントした話
あらまし
今日始めて近くの3Dプリントサービスを使ってきました。
プリントしたのはラズパイマウスのcameraマウンタです。 DMMmakerさんで高精度にプリントされたものが2700円で売ってます。
これを自前で印刷してみようと,近所の工房に来て印刷しました。 お値段は810円でした。(失敗して二度印刷しましたがまけてもらいました^^)
所要時間:28分 \ 積層厚さ:2.8mm \ サポート材:あり(なしで失敗)\ 費用:810円(税込)
3Dプリント時の注意点
一応今回のプリントする時の注意みたいなのをメモします。 超基礎事項ですので経験者は飛ばしてみてください。
- 向きに気をつける必要がある
- サポート材は基本ありでやる
向きとは例えばネジ穴などの箇所は穴の中心線に対して垂直にしたほうがいいとかそういうことみたいです。 成形物は鉱物のように積層方向に割れやすいので穴に対して縦に裂けるのを避けるという意図があるそうな。
また,置く向きによって支えが必要になるのでそのことも考えて設計・配置しなければいけないみたいですね。
stlファイルの分割
該当のstlファイルはただで配っているのでそのまま落としてくればいいんですが,部品がくっついていてどうも向きなどの調整が難しいので分離してしまおうと思いました。
工房のPCにはMeshmixerというソフトが入っていたのでそれをつかってやったことをメモします。 基本的には下記のサイトに書いてあることに倣えばよさそうです。
- 選択ツールでつなぎ目を選択
- 編集ツールで削除
- 余計な箇所も削れるので解析ツールで解析して修復
- 選択ツールで部品を選択,エクスポートを行う
- 選択後の編集ツールの「分離」という操作でもオブジェクトを分解できるみたい
とりあえず何もわかってませんが分離自体はできました。
分離したファイル
分離したファイルをここに配布します。
プリントしたところ,cameraに接続する側の正方形の部品が膨らみ気味ではまらなかったので精度を上げるか,プリント後に削るかしたほうが良いでしょう。
プリントした結果
一回目(サポート材無し)
二回目(サポート材あり)
サポート材を入れないと浮いている箇所は当然失敗します。そらそうや。
後始末
プリント後のサイズが若干合わなかったのでちょっと削ります。ヤスリでやったけどちょっと不格好かも。
所感・まとめ
- 自前で3Dプリントやるのは結構手間がかかるのとそんなに精度が出ない
- お金と時間に余裕があるならDMM等に頼んでも良さそう
MATLABでnumpyで保存したcsvファイルを開く
結論
書きはじめの時は,numpyで保存したcsvがmatlabに変な感じで認識されるのを不満に思っていたのですが,途中から自分の過ちに気づきました.
要点は以下の2つです.
- 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
みんなもちゃんとリファレンス読みましょう.
ROSBagから画像を抽出する(image_viewを使うべきではない気がする)
問題設定
実験などのデータをrosbag形式で保存して静的な環境で検証をしたいということが多々有るかと思います.
image_viewを用いて画像をエクスポート
image_viewのexport imageを使う(非推奨)手法があります.
公式?のDocumentを元に進めればOKです.
以下のexport.launchと言う名前のlaunchfileを作って,実行せよとのことです.
<launch> <node pkg="rosbag" type="play" name="rosbag" args="-d 2 $(find image_view)/test.bag"/> <node name="extract" pkg="image_view" type="extract_images" respawn="false" output="screen" cwd="ROS_HOME"> <remap from="image" to="/camera/image_raw"/> </node> </launch>
$(find image_view)/test.bag
のところを対象のbagファイルに変更すればOKです.
roslaunch export.launch
いい点
- launchファイルを叩くだけで簡単にできる
- 多分変数をいじればもっと細かい指定もできそう.
悪い点
- timestampの情報を拾うことができない(多分)
- PCのスペックなどにより,拾い落としが起きる場合がある
後の手法を用いた方が余すことなくデータを取ることが出きます.
やっていることのメモ
rosbagのplayの後に-d 2
という引数がついていますが
rosbagのrefに拠ると,2秒開始するのを待つということのようです.(ノードに接続するため)
PythonのrosbagAPIを叩く方法(推奨)
先ほどのは本番環境を模擬して試験する文には良いのですが,dataのConnection Lossが起きうるので記録したデータを解析したいという目的には不向きだと思います.
rosbagファイルから特定のメッセージを抽出
rosbagをインポートします.今回はたまたま画像の解析をしたいので必要なパッケージは以下のようになります.
import rosbag import cv2 from cv_bridge import CvBridge, CvBridgeError from sensor_msgs.msg import Image
以下のようにBagモジュールのread_messages()というメソッドを用いてトピック名,メッセージ本体,タイムスタンプを入手できます.
timestamps = []; images = []; for topic, msg, t in rosbag.Bag('HOGEFUGA.bag').read_messages(): if topic == '/zed/left/image_raw_color': timestamps.append(t.to_sec()) images.append( CvBridge().imgmsg_to_cv2(msg, "8UC3") )
- timestampはros.Timeという形式なので to_sec()で変換する
- メッセージの変換などは通常のrospyファイルと同様
おまけ:比較用ビデオ生成コード
ビデオにすると先ほどの手法と比較してどの画像が落ちていそうか見ることができます.
## Create Video for Evaluation size = (images[0].shape[1],images[0].shape[0]) out = cv2.VideoWriter('bagvideo.avi',cv2.VideoWriter_fourcc(*'DIVX'), 15, size) for i in range(len(images)): out.write(images[i]) out.release()
Python Gmail API を用いてメールの締切を自動でリストアップする①
あらまし
最近謎にものづくりのモチベがあります。昨日メール見ていて締切多すぎぃ!ってなったのでメールから自動で締切をリストアップしてくれる手法がなんかないかなと思ってGmail APIのドキュメントとにらめっこしてきました。
目指したい理想像
- Gmail に届いているメールから「締切」に関係するワードを抽出
- 同メールから「日付」情報を抽出
Gmail APIを用いたメール抽出
まずはQuickStartのガイドから入っていきます。
ここではまず,Googleの情報にアクセスする認証キー的なものをダウンロードできます。 キーの仕様についてはこっちを参照すると良いでしょう。
APIの立ち上げ
上の手順でquickstart.py
というサンプルコードを実行すると思います。
実質的なサービスの立ち上げは,以下の箇所にあるようです。
service = build('gmail', 'v1', credentials=creds)
ここでcredentialsというのが初回の登録でDLしたcredential.json
というAPIの認証キーです。
build関数の仕様に依るとgmail
のサービスを'v1'のバージョンで起動するということみたいですね。
Queryとマッチングするメッセージを検索
メールの検索ですがAPIがきちんと準備されています。
必要なものはだいたい以下の通りです。
- 先程立ち上げた
service
- ユーザID :メールアドレス or 'me'(自分自身のこと)
- 検索クエリ:検索文字列。ここでは「締切」などとする
- maxResults:検索上限。時間を気にするなら入れておくといい。
messages
に帰ってくるのはメール情報のidの配列です。
response = service.users().messages().list(userId=user_id, q=query,maxResults=maxlistnum).execute() messages = [] if 'messages' in response: messages.extend(response['messages'])
サンプルコードでは nextPageToken
なるものを使ってますが,これは構造体や配列を辿る時の現在地を表すポインタ的なやつです。
idからメッセージの本文などを抽出
先程の検索ではメッセージのidを入手したのでget関数を用いてメッセージの中身を取得します。
ここのサンプルコードの困った点はマルチバイト文字が含まれているときにエンコーディングの問題が発生するということです。
APIの記述的にはMIMEという形式の方を推していそうですが,メールに依って文字化けをしたりしなかったりして半日では解決できなかったので今回は採用を見送りました。
それで代わりに書いた関数がこれ。(クラスの形式になってるけど直すの面倒なので各自解釈してください)
def GetMessageSubjectAndBody(self, msg_id): """Get a Message and use it to create a MIME Message. Args: msg_id: The ID of the Message required. Returns: A MIME Message, consisting of data from Message. """ try: message = self.service.users().messages().get(userId=self.user_id, id=msg_id, ).execute() body = "" subject = "" if message['payload']['body']['size']: msg_str = base64.urlsafe_b64decode(message['payload']['body']['data']) body = msg_str.decode('utf-8') if 'headers' in message['payload']: for header in message['payload']['headers']: if header['name'] == 'Subject': subject = header['value'] return subject, body except errors.HttpError as error: print('An error occurred: %s' % error)
基本的にはmessage['payload']['body']['data']
の中に本文がありますが,稀にこのデータが空の場合があります。今の所そういうのは真面目なメールではなかったので例外処理をして回避しています。
また,同様にmessage['payload']['header']
の中に件名を保管している箇所があるのでそこも掘り起こしています。気になる方は一度Jupyterなどで中身を見てみると良いでしょう。
提案アルゴリズム
上記の手段でメールの本文を検索して見ることができるようになりました。 これをどのように使っていくかについて書いていきます。
締切に関する語句の検索(メール検索時)
締切と言っても様々な表記があります。その全てを検索クエリに含める必要があるのですが,その時の書き方は以下のようにORでつなぐことで実現します。
"締め切り OR 〆切 OR 締切 OR 締切り OR deadline OR DEADLINE"
もちろん,普段gmailの検索に用いるクエリも使用可能で,ここにあるように,
重要なメールであれば ”AND is:important”
とかつけて検索することもできます。
締切の日時の検索(メール内検索)
メール本文を抽出した後,締切に関する日時を検索します。日時というのは意外とメールの中で自然に使われているので"締切"などのワードの近くを検索したいです。
従って,
- 締切に関連する語句の付近の文章抽出(行単位or文字数で)
- 抽出した文章から日付の抽出
という手順を取りました。
日付の抽出には非常によく整備されたツールがあるのですが,残念ながら日本語対応しておらず,2バイト文字に対して誤作動的なのを起こすので今回は採用をやめました。 もし使うなら2バイト文字があるかどうか判別してから用いればよいかと思います。海外の方と多く連絡するのであれば導入も考えます。(今のところはそんなにない) github.com
代わりに以下の正規表現を用意しreモジュールを用いて検索をかけています。(正規表現初めて使ったので深く理解していない)
['(\d{1,2})/(\d{1,2})', '(\d{1,2})月(\d{1,2})日']
この表現を用いることで「3/4」や「7月4日」といった表現をキャッチできます。半角全角にかかわらずキャッチできるので偉い。
まとめてデータ化
以上の手順で,締切に関する語句を含んだメールから締切の日時と件名を抽出できました。
これをまとめてデータ化してGoogleカレンダーにぶち込むなどしたいですが,ちょっと息切れしたので辞書型に詰めてひとまず完成とします。
ちょっと見せるとしうかつだらけですね。なおIDは加工しました。検索できないよ。 見学会系はちょっとたくさん締切を発行してきますね。他の用途にも使えそうなので今回遊んでみてまぁまぁよかったんじゃないかなと思ってます。
{'deadlines': [[3, 18]], 'message_id': '1697f3bcab', 'subject': '【3月18日(月)午前9時締切】ソニー・インタラクティブエンタテインメント\u3000' '技術系・事務系エントリーシート受付中'}, {'deadlines': [[3, 8], [4, 3], [3, 8], [5, 13]], 'message_id': '1695b47b1', 'subject': '【日本精工(株)】ログインのお願い:【総合職】応募受付開始しました。'}, {'deadlines': [[3, 1]], 'message_id': '16933597b7', 'subject': '【重要なお知らせ】3月1日0時より『まとめてプレエントリー』機能をご利用いただけます'}, {'deadlines': [[3, 1]], 'message_id': '16923774f', 'subject': '【重要】2/27(水)~2/28(木)プレエントリー受付開始準備による一部機能停止のご案内'}, {'deadlines': [[1, 31]], 'message_id': '168944a8b6', 'subject': ' 【特別号】有効期限が近づいているマイルやe JALポイントはございませんか? / 【先得】 ' 'JMB会員先行予約サービス 受付間もなく開始!'}, {'deadlines': [[1, 31]], 'message_id': '16886a4406', 'subject': '【UTAS】(周知)【1/31まで deadline Jan ' '31】情報セキュリティ教育について/e-learning(Information Security ' 'Education)'}, {'deadlines': [[1, 23]], 'message_id': '1686095a6', 'subject': '【ご好評につき2期募集開催!】マーケティング、商品企画などコースに特化した短期インターンプログラム"Business ' 'Master Program"'}, {'deadlines': [[1, 31]], 'message_id': '1683af6025', 'subject': '【2019年1月31日締切】 JAL旅行積立 ボーナスマイルキャンペーン実施中!'}, {'deadlines': [[1, 8], [1, 8]], 'message_id': '16827b89', 'subject': '【締切1/8(火)10:00】(事務系・技術系)ソニー インターンシッププログラム エントリー受付中'},
今後やりたいこと(誰かにやってほしいこと)
今後の課題は,
- メールのid,件名,抽出した締切 を含んだ適切なデータ構造
- 件名や締切で検索をかけることができ,APIを叩いて本文全文を参照できるようにする
- Google Calendarなどに見やすく一覧でマッピングする
- 重要な締切かどうかまで判別してくれるとめちゃ助かる
といったところでしょうか。 コード公開したら誰か引き継いでくれたりしないだろうか。