2019-07-19

微小回転に関する覚書：微小回転行列近似 VS クォータニオン

数学 MATLAB

メモです。余裕のあるときに後でリファインします。

前提
- 微小回転行列
- クォータニオンによる角速度の積分
MATLABで比較プログラム
- 問題設定①
  - 微小回転行列の作成
  - クォータニオンの計算
- 問題設定②
自前関数
参考文献

前提

座標系が角速度 $\omega = (r,p,y)^\top$ で回転しているとする。

微小回転行列

本来三次元回転SO3は非可換であるが，微小回転を考えればこれを可換にすることができる。1

先程の角速度で微小回転行列というものが考えられており， $\omega \Delta t = (\alpha,\beta,\gamma)^\top$ とすれば以下のような回転行列で表せる。

f:id:ossyaritoori:20190719013841p:plain

これは以下に示すcross product表記を用いれば， $R= \omega\Delta t_{ \times }+eye$ と表すことができる。

f:id:ossyaritoori:20190719014129p:plain — wikipediaより引用：歪対称行列

クォータニオンによる角速度の積分

クォータニオン便利計算ノートによればクォータニオンによる積分は以下の通り。

f:id:ossyaritoori:20190719014911p:plain — 一番下の式には1/2が入っていないと思う。見落としがなければ。

先程のcross productを用いて表すなら，

$$ \dot{q}=\frac{1}{2} \begin{pmatrix} 0 & -\omega^\top\\ \omega & \omega_{\times} \end{pmatrix} q $$

と書いたほうがスッキリするだろう。

MATLABで比較プログラム

P.I.corke先生のRobotics Tool boxを使用。課金できる方は自前でToolboxを買うと良い。なお，2019年以降に東大に在籍している学生は無料で全ライセンス使えるのでぜひ活用するとよい。

petercorke.com

昔は全機能使えたと思ったのですが今入れたらいくつかの機能が削除されていたような？？？

問題設定①

最初の角度をオール0とする。同次変換行列を作っておく。

orig_eul = [0,0,0]'
orig_tr = rpy2tr(orig_eul')

omegaを適当に定め，10ms積分するとする。

omega = [0.1,0.2,0.3];
dt = 0.01;

微小回転行列の作成

cross productを計算するskewという関数を用いて簡単に作成可能。

R1=skew(omega*dt)+eye(3)

TR1 =r2t(R1)*orig_tr

TR1 =

    1.0000   -0.0030    0.0020         0
    0.0030    1.0000   -0.0010         0
   -0.0020    0.0010    1.0000         0
         0         0         0    1.0000

クォータニオンの計算

回転操作などをクォータニオンに変換すればよいが，新しく落としたToolbox：10.3.1はクォータニオンにまつわる変換がいくつか取り去られていた。しょうがないので自分で書いた。

orig_q = eul2quat_self(orig_eul)
P2q = integratequat(orig_q,omega*dt)
TR2 = rpy2tr(quat2eul_self(P2q))

TR2 =

    1.0000   -0.0030    0.0020         0
    0.0030    1.0000   -0.0010         0
   -0.0020    0.0010    1.0000         0
         0         0         0    1.0000

問題設定②

最初の角度を適当に決める。ロールピッチヨーのオイラー角を適当に決める。うん。後でやります。

orig_eul = rand(3,1)

orig_eul =

    0.9134
    0.6324
    0.0975

TR1 =

    0.8014    0.4054    0.4398         0
    0.0816    0.6543   -0.7518         0
   -0.5926    0.6384    0.4913         0
         0         0         0    1.0000

TR2 =

    0.8014    0.4054    0.4398         0
    0.0816    0.6543   -0.7518         0
   -0.5926    0.6384    0.4913         0
         0         0         0    1.0000

結果がほぼ同じになるのは違和感がある。前者は1次近似で後者は変換部を除けば理想的な計算結果になると思っていたので。誤差は1e-5のオーダ。

 TR1-TR2
 =

   1.0e-05 *

    0.7539   -0.0609    0.1604         0
    0.1528    0.0783   -0.5804         0
   -0.0849    0.0234    0.3760         0
         0         0         0         0

自前関数

こちらの記述を大部分引用。

参考文献

あとで♡

2019-07-12

MATLAB symbolic mathを使って煩雑な手計算を自動化する

MATLAB コンピュータビジョン

座標変換などで手計算をあまりしたくないのでsymsをうまく使ってこれを回避する方法をメモします。

Image Jacobianの導出
まとめ
- prettyコマンドの活用
参考文献

Image Jacobianの導出

カメラの座標系から見て位置（X,Y,Z）にある物体を考えます。焦点距離fとすると，以下の関係が成り立ちます[1]。

f:id:ossyaritoori:20190712145950p:plain — カメラの撮像幾何[1]

カメラが6軸の速度・加速度を持つときに画像内の点がどのように動くのか，その関係性を表す行列を導出します。

f:id:ossyaritoori:20190712150807p:plain — 何故かうさぎをアクチュエータに使うPeter先生。かわいい。

時間変化する変数を定義

時間tに関する関数として位置の各変数を定義できます。[2]の橋本先生の論文を例にとって見てみましょう。

f:id:ossyaritoori:20190712151433p:plain — ピンホールカメラモデルによる撮像の式（ただしf=1として省略）

syms t X(t) Y(t) Z(t) 
image_p = [X(t); Y(t)]/Z(t)

計算結果：

image_p =
 
 (X(t))/Z(t)
 (Y(t))/Z(t)

時間で偏微分

tで偏微分します。

f:id:ossyaritoori:20190712151618p:plain — 商の微分

diff_p = diff(image_p,t)

こうなります。

diff_p =
 
 diff(X(t), t)/Z(t) - (X(t)*diff(Z(t), t))/Z(t)^2
 diff(Y(t), t)/Z(t) - (Y(t)*diff(Z(t), t))/Z(t)^2

特定の数式の代入による式整理

先程の式のXdotなどをvxに変換していくために以下の数式を代入します。

f:id:ossyaritoori:20190712151743p:plain — pdot=-v+w×p から計算

subs(対象式, 代入対象, 代入する式)という構文を用いて代入。 diff(X(t), t)という式を消していきます。

syms vx vy vz wx wy wz

before={diff(X(t), t),diff(Y(t), t),diff(Z(t), t)};
after = { -vx-wy*Z(t)+wz*Y(t), -vy-wz*X(t)+wx*Z(t), -vz-wx*Y(t)+wy*X(t)};

diff_p_XYZ = subs(diff_p,before,after)

結果，以下のようになります。

 (X(t)*(vz - wy*X(t) + wx*Y(t)))/Z(t)^2 - (vx - wz*Y(t) + wy*Z(t))/Z(t)
 (Y(t)*(vz - wy*X(t) + wx*Y(t)))/Z(t)^2 - (vy + wz*X(t) - wx*Z(t))/Z(t)

小手先の代入テク

ただ，元の式展開では，[x,y]=[X/Z,Y/Z] の関係を用いてX,Yの変数を消去しています。これもsubsで計算可能です。

syms x y

before={X(t), Y(t)};
after = { x*Z(t), y*Z(t) };

diff_p_xy = subs(diff_p_XYZ ,before,after)

 (x*(vz - wy*x*Z(t) + wx*y*Z(t)))/Z(t) - (vx + wy*Z(t) - wz*y*Z(t))/Z(t)
 (y*(vz - wy*x*Z(t) + wx*y*Z(t)))/Z(t) - (vy - wx*Z(t) + wz*x*Z(t))/Z(t)

上では明らかにZで約分できるにもかかわらず約分されていないのが気になります。小手先テクですがsimplify関数を適用することで整理が可能です。

diff_p_xy = simplify(subs(diff_p_XYZ ,before,after))
 
diff_p_xy =
 
wz*y - vx/Z(t) - wy - wy*x^2 + (vz*x)/Z(t) + wx*x*y
 wx - vy/Z(t) - wz*x + wx*y^2 + (vz*y)/Z(t) - wy*x*y

f:id:ossyaritoori:20190712153325p:plain — 展開結果

多項式から行列形式への変形

最終的には画像内の点の動きd[u,v]/dtとカメラの動きの対応を以下のように行列形式で扱います。その時の変換行列が画像ヤコビ行列というやつです。

f:id:ossyaritoori:20190712153526p:plain — 画像ヤコビ行列

先程求めたのは多項式ですので，equationToMatrixという関数を使います。

% 便宜上の数式変換なのでぶっちゃけ右辺は何でも良い
eqns = [diff_p_xy == [0;0]];

% 抜き出す変数
vars = [vx vy vz wx wy wz];

% 係数行列（ヤコビ行列の抽出）
J = equationsToMatrix(eqns,vars)

結果がこちら。計算ミスなく簡単に導出できますね。

J =
 
[ -1/Z(t),       0, x/Z(t),     x*y, - x^2 - 1,  y]
[       0, -1/Z(t), y/Z(t), y^2 + 1,      -x*y, -x]

まとめ

このように式をこねくり回すのもMATLABに任せれば良いと言いたいところですが，結局計算ミスの代わりにプログラムのデバッグが必要になります。特に添字などの間違いは見逃しやすいのでうまいこと定義した数式を見やすくするなどしてデバッグのコストを下げたいところです。

symbolicを直接latexに変換するといったプラグインも開発されてたりするそうなのでうまいこと活用して楽ができればいいですね。

prettyコマンドの活用

数式を少し見やすくするためのコマンドが一応用意されています。

これ本当に見やすいかな？？？

>> pretty(diff_p)
/  d              d      \
| -- X(t)   X(t) -- Z(t) |
| dt             dt      |
| ------- - ------------ |
|   Z(t)            2    |
|               Z(t)     |
|                        |
|  d              d      |
| -- Y(t)   Y(t) -- Z(t) |
| dt             dt      |
| ------- - ------------ |
|   Z(t)            2    |
\               Z(t)     /

参考文献

[1] P.I.Corke先生のYotube解説より抜粋 url

[2] 橋本先生の「Visual Servo - IV - Feature Based Visualservo（ビジュアルサーボ：特徴ベースビジュアルサーボ）」より抜粋

2019-07-10

2019年版Overleaf v2 日本語Tex環境テンプレ

環境概要
日本語環境の構築
機能活用
- キーボードショートカット
- Github等連携

環境概要

以前OverleafとSharelatexの紹介などをしたが，いつの間にか統合されてOverleaf v2になっているようだ。オンライン環境がある場合余分にソフトを起動しなくて済むため比較的使用感は良かった（TexLive2017あたりが入っている？）。

日本語環境構築
Github連携

日本語環境の構築

最初にやることは， - Menuを開いてデフォルトのTex コンパイラを”Latex”へと変更

具体的な画面の状況や操作などはここの記事にある。

ファイル構造

文章は最初から作成してあるmain.texを編集していく。

最低限のものを詰め込むと以下のようなファイル構成になると思う。

.
├ fig/ （図とか）
├ latexmk（コンパイル設定）
├ main.tex （本文）
├ preamble.tex （プリアンブル）

ディレクトリ構造のトの字ってどうやって出すんだ？

latexmkの作成

latemxmkでコンパイルの方法を設定する。

$latex = 'platex';
$bibtex = 'pbibtex';
$dvipdf = 'dvipdfmx %O -o %D %S';
$makeindex = 'mendex %O -o %D %S';
$pdf_mode = 3;

preamble.texの作成

これはお好みで良い。自分用のものを以下に記す。

\usepackage[dvipdfmx]{color,graphicx}
\usepackage{ascmac}
\usepackage{amsmath,amssymb}
\usepackage{epsf}
\usepackage{float}

\newcommand{\bm}[1]{\mbox{\boldmath $#1$}}
\newcommand{\eq}[1]{Eq.(\ref{eq:#1})}
\newcommand{\zu}[1]{図\ref{fig:#1}}
\newcommand{\shiki}[1]{（\ref{eq:#1}）}
\newcommand{\fig}[1]{Fig.\ \ref{fig:#1}}
\newcommand{\grp}[1]{Fig.\ \ref{grp:#1}}
\newcommand{\tab}[1]{Table\ \ref{tab:#1}}
\newcommand{\defeq}{:=}

\newcommand{\mr}[1]{\mathrm{#1}}%rm体
\newcommand{\ml}[1]{\mathcal{#1}}%frac体
\newcommand{\bmat}[1]{\begin{bmatrix} #1 \end{bmatrix}}%行列簡略化
\newcommand{\pmat}[1]{\begin{pmatrix} #1 \end{pmatrix}}%行列簡略化
\newcommand{\mat}[1]{\left( \begin{matrix} #1 \end{matrix} \right)}%行列簡略化
\newcommand{\expo}[1]{ \mathrm{e}^{#1} }

本文の設定

使う論文のテンプレ次第でいかようにもなるが，先程のpreambleをinputで読み込ませるとスマートな見た目になる。余白の調整はgeometryパッケージを使うと良い。

\documentclass[10pt,a4paper,oneside,twocolumn,fleqn,dvipdfmx]{jsarticle}
\usepackage[utf8]{inputenc}
\input{preamble.tex}

\usepackage{geometry}
\geometry{left=25mm,right=25mm,top=25mm,bottom=25mm}

%本文
\begin{document}

\twocolumn[
    \begin{flushright} 
        右上枠
    \end{flushright}
    \centering{
        \LARGE{タイトル}
    }
    \section*{
        \centering{Abstract}
    }
    \raggedright{
    Write abstract here.
    }
    \vspace{2ex}
]

\section{Background}

%参考文献
\small
\bibliographystyle{IEEEtran}
\bibliography{ref} %ref.bibを準備
\normalsize

\end{document}

機能活用

キーボードショートカット

コピペなどに関しては基本的なDOS用のコマンドがいくつか使える。他にはコメントアウトのトグルであるCtrl+/などが有用である。

以下の画像にまとめてある。Macではない機能もあるらしいが使ったことはないしまぁ問題ないかも。

Github等連携

GithubやDropbox等と連携することで管理がしやすくなる。月額なんと15ドル！~~ちょっと高いのでやらない（）~~

その他便利環境に関してはこの記事によくまとまっている。

2019-06-10

ASUS Zenbook Pro UX550VD の分解清掃を試みた話

コンピュータガジェット

何がおきたのか
- 該当商品について
- 1年間使ってみて
必要なもの
分解工程
- 裏面のネジを取る
  - 裏蓋の取り外し（追記更新）

結論から申し上げるとネジの箇所を把握しきれず完全分解には至りませんでした。失敗記事なのですがそもそも本機体の分解に関する記事がネット上にないので情報共有のために残しておくことにします。

何がおきたのか

15ヶ月該当商品を使っていましたが，最近ファンの音が大きく感じるようになりました。

ぼちぼち掃除が必要だなと感じたので分解清掃を試してみます。

該当商品について

2017年に発売開始されたZenbookのフラグシップ機です。

特徴として，大きく重い（15.6インチ 2.4kg）代わりに，Geforce GTX 1050を積んでRAMが16Gがある本格仕様になっております。一年前に買った時の感想などが載っていますね。

ossyaritoori.hatenablog.com

以下はCorei5版ですが今はかなり安く売られているようです。

UX550VD-7300(ブル-) ZenBook Pro UX550VD 15.6型液晶

メディア:

1年間使ってみて

良かった点として

性能で困ることはあまりなかった
電池のもちも結構良い

欠点として - とにかく重い - トラックパッドが少し使いにくい（当人の慣れかも）

稀にマウス操作がカクつくなどの現象が置きましたが，ハード由来かソフト由来か不明なので不問とします。

必要なもの

トルクスネジという星型のネジを使っているので普通のドライバーでは開けません。六角が入らない！形の違う「トルクス」というねじ | Cyclist

型としては

T5ドライバー

を探しましょう。

私は以下のものを買いました。

Kingsdun トルクスドライバードライバー T5 ヘクスローブドライバー精密ドライバー星型ドライバー六角ドライバー特殊ドライバートルクス磁石付きドライバー MacBook Pro/Retina/Air Nexus4/HDD/SSD/PC/パソコン分解、交換、修理用ドライバーメンテナンス工具ドライバー

メディア:

分解工程

裏面のネジを取る

SSD換装動画ですが以下の動画が参考になります。

www.youtube.com

この際，おすすめなのが両面テープやガムテープを用いてネジの元の位置を保存しておくことです。本機体は全てのネジが同じ形式っぽかったので問題なかったですが，別のPCだとネジの長さが違ったりするのでとても重要な作業です。

裏蓋の取り外し（追記更新）

機種依存ですが今回チャレンジした感じ的にはゴム足の下にもネジがあるように感じました。がこれは気のせいでした。 www.zoa.co.jp

ここからは裏面を剥がすのですがこれは力押しでやると曲がったりすることがあるので，樹脂製の薄いピックやプラスチックカードなどを隙間に入れて徐々に開けるのが良いです。

f:id:ossyaritoori:20200927025328j:plain

うまく開けばこんな感じに中身が見れます。この中を清掃します。やり方などについてはまた後ほど。

2019-05-22

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

カルマンフィルタ matlab

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

カルマンフィルタの基礎

作者: 足立修一,丸田一郎
出版社/メーカー: 東京電機大学出版局
発売日: 2012/10/10
メディア: 単行本（ソフトカバー）
購入: 3人クリック: 3回
この商品を含むブログ (5件) を見る

線形カルマンフィルタの終端値とリカッチ方程式
- MATLABでリカッチ方程式を解く解法
誤植箇所
- p149 演習６-２の解答

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

線形カルマンフィルタの事前推定の共分散を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前後が正解の値になるはずです。なお，スカラなので手計算でリカッチ方程式を解いても同様の値になります。

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

2019-05-06

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

jupyter

notebookのセル出力を横にしたい
- 設定方法

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のページでは普段どおりに写ります。