Q&A
ekfとか関係なく、"実際のロボットの位置"がおかしいですよね？
MATLABを用いてカルマンフィルタをによる自律型ロボットの自己位置推定のプログラミングを学んでいるものです。
自分の実装しているコードの特徴として、
・ロボットへの入力が速度と角度（ｖ、θ）などではなく、現在の位置から目標位置への引力（速度ベクトル）V =（Vx,Vy）を採用していることです。状態遷移式としては、X(t+1) = FX(t)+aV のようになります。（aは降下量係数）
・観測データは、1点をランドマークとして設置し、現在の位置からランドマークへの距離と角度をデータとして得られる設定にしています。
困っていることとしては、途中まで正しく推定できている位置が、ランドマークとの距離が最小になるステップ以降、暴走してしまうことです。
（図参照）青い点：実際のロボットの位置、赤い点：カルマンフィルタによる推定の位置、緑の点：ロボットが目指す目標位置、黒い点：ランドマーク 青い楕円：推定誤差楕円 です。
おそらく、観測データの中の角度の処理（atan2）がうまくいってないと思われるのですが、なかなか解決できません。
カルマンフィルタに詳しい方、ぜひアドバイスをいただきたいと思います。
ソースコードも載せておきますが、あまりきれいなコードではないと思います、、、
この方のページをよく参考にさせていただいています。↓
https://myenigma.hatenablog.com/entry/20130413/1365826157
function [] = VectorFieldKalmanFilterLocalization() close all; clear all; disp('Vector Field Kalman Filter sample program start!!') global xg xg = [20;20]; global cg cg = 300; global lg lg = 100; global a a= 0.1; x0=[0;0]; global LM LM= [15;10]; %plot(x0(1),x0(2),'o','MarkerSize',10,'MarkerFaceColor','y');hold on; plot(xg(1),xg(2),'o','MarkerSize',10,'MarkerFaceColor','g');hold on; plot(LM(1),LM(2),'o','MarkerSize',15,'MarkerFaceColor','k');hold on; result.xTrue=[]; result.xd=[]; result.xEst=[]; result.z=[]; result.PEst=[]; result.u=[]; Xact=[0;5]; Xdes=[0;0]; Xest=[0;5]; global z z = [0;0]; global F F=eye(2); H=eye(2); Pest = eye(2); w=[0.01;0.02]; v=[0.03;toRadian(1)]; Q=diag(w).^2; R=diag(v).^2; % Main loop for t=1:300 %目標軌道ベクトル %Vdes = VerocityVector(Xdes); %目標軌道 %Xdes = f(Xdes,Vdes); %実軌道ベクトル Vact = VerocityVectorIncludeNoise(Xact,w); %実軌道 Xact = f(Xact,Vact); %予測軌道ベクトル Vest = VerocityVector(Xest); %予測軌道と共分散 Xpred = f(Xest,Vest); [vxdx vxdy vydx vydy] = bibun(Vest,Xest,xg); Ft = jacobF(vxdx,vxdy,vydx,vydy); Ppred = Ft*Pest*Ft' + Q; %観測値 z = Observation(Xact,LM,v); zpred = Observation(Xpred,LM,v*0); %カルマンゲイン計算 y = z - zpred; Ht = JacobianH(Xpred,LM); S = R + Ht*Ppred*Ht'; K = Ppred*Ht'*inv(S); %更新値 Xest = Xpred + K*y'; Pest = (eye(2) - K*Ht)*Ppred; figure(1); set(gca, 'fontsize', 16, 'fontname', 'times'); %plot(Xdes(1), Xdes(2),'.k','linewidth', 7); hold on; plot(Xact(1), Xact(2),'.b','linewidth', 7); hold on; plot(Xest(1), Xest(2),'.r','linewidth', 7); hold on; title('EKF Localization Result', 'fontsize', 16, 'fontname', 'times'); xlabel('X (m)', 'fontsize', 16, 'fontname', 'times'); ylabel('Y (m)', 'fontsize', 16, 'fontname', 'times'); grid on; axis equal; if rem(t,5)==0 ShowErrorEllipse(Xest,Pest); end drawnow; end function ShowErrorEllipse(Xest,Pest) %誤差分散円を計算し、表示する関数 Pxy=Pest(1:2,1:2);%x,yの共分散を取得 [eigvec, eigval]=eig(Pxy);%固有値と固有ベクトルの計算 %固有値の大きい方のインデックスを探す if eigval(1,1)>=eigval(2,2) bigind=1; smallind=2; else bigind=2; smallind=1; end chi=9.21;%誤差楕円のカイの二乗分布値 99% %楕円描写 t=0:10:360; a=sqrt(eigval(bigind,bigind)*chi); b=sqrt(eigval(smallind,smallind)*chi); x=[a*cosd(t); b*sind(t)]; %誤差楕円の角度を計算 angle = atan2(eigvec(bigind,2),eigvec(bigind,1)); if(angle < 0) angle = angle + 2*pi; end %誤差楕円の回転 R=[cos(angle) sin(angle); -sin(angle) cos(angle)]; x=R*x; plot(x(1,:)+Xest(1),x(2,:)+Xest(2)) function V = VerocityVector(x) %位置xにおける速度ベクトルの計算 global xg; global cg; global lg; Vx = -(2*cg*(x(1)-xg(1))/lg^2)*exp(-(((x(1)-xg(1))^2)+(x(2)-xg(2))^2)/lg^2); Vy = -(2*cg*(x(2)-xg(2))/lg^2)*exp(-(((x(2)-xg(2))^2)+(x(2)-xg(2))^2)/lg^2); V = [Vx;Vy]; function V = VerocityVectorIncludeNoise(x,w) %位置xにおけるノイズを考慮した速度ベクトルの計算 global xg; global cg; global lg; Vx = -(2*cg*(x(1)-xg(1))/lg^2)*exp(-(((x(1)-xg(1))^2)+(x(2)-xg(2))^2)/lg^2)+w(1)*randn; Vy = -(2*cg*(x(2)-xg(2))/lg^2)*exp(-(((x(2)-xg(2))^2)+(x(2)-xg(2))^2)/lg^2)+w(2)*randn; V = [Vx;Vy]; function x1 = f(x,v) %動作モデル global a; x1 = x + v*a; function z = Observation(x,xm,v) %計測モデル z(1) = ((xm(1)-x(1))^2+(xm(2)-x(2))^2)^0.5 + v(1)*randn; z(2) = PI2PI(atan2(xm(2)-x(2),xm(1)-x(1)))+ v(2)*randn; %z(1) = x(1)+v(1)*randn; %z(2) = x(2)+v(2)*randn; function angle=PI2PI(angle) angle=atan2(sin(angle),cos(angle)); function Ft = jacobF(vxdx,vxdy,vydx,vydy) % Jacobian of Motion Model global F; global a; Ft=F+a*[vxdx vxdy;vydx vydy]; function Ht = JacobianH(x,LM) Ht = [ -(LM(1)-x(1))/((LM(1)-x(1))^2+(LM(2)-x(2))^2)^0.5, -(LM(2)-x(2))/((LM(1)-x(1))^2+(LM(2)-x(2))^2)^0.5; -(LM(2)-x(2))/((LM(2)-x(2))^2+(LM(1)-x(1))^2), -(LM(1)-x(1))/((LM(2)-x(2))^2+(LM(1)-x(1))^2)]; function [vxdx vxdy vydx vydy] = bibun(v,x,xg) global a; global cg; global lg; vxdx = -((2*cg*a)/lg^2)*(exp(-(((x(1)-xg(1))^2)+(x(2)-xg(2))^2)/lg^2))*(1+((x(1)-xg(1))*v(1))/2*a*cg); vxdy = -v(1)*(2*(x(2)-xg(2))/lg^2); vydx = -v(2)*(2*(x(1)-xg(1))/lg^2); vydy = -((2*cg*a)/lg^2)*(exp(-(((x(1)-xg(1))^2)+(x(2)-xg(2))^2)/lg^2))*(1+((x(2)-xg(2))*v(2))/2*a*cg); function radian = toRadian(degree) % degree to radian radian = degree/180*pi; ```
実際のロボットの位置は、初期位置[0,5]から
X(t+1) = X(t)+0.1*V
によって目標位置[20,20]に進む制御をしています。
青い点で示される経路は結果画像からも正しいととれるのですが、、、
どのあたりがおかしいですか？
青が真の座標ですよね？
x=17付近でyが飛んでますけど
このロボはワープでもするんですか？
それとも見方が違うんでしょうか
あ、誤差円ですね
失礼しました
あ、その通りです！
真の座標と誤差楕円の色を同じままにしてしまったため、誤解を招いてしまい申し訳ないです。
閲覧いただき、ありがとうございます。また何かありましたら、連絡お願いします。
Ht*xって観測になります？
はい！
実際の観測値:z=Ht＊Xact+(ノイズ)
観測値の予測:zpred=Ht＊Xpred
となります。
自分も今回の問題について調査中ですが、おそらく、カルマンフィルタの更新部分で、カルマンゲインKにかかわる行列(HtやPpred,Sなど)の値が安定していないため、なにか修正が必要なのかもしれないと、感じてきました。
お時間があれば、よろしくお願いします。
いや、要するにxが直交座標でzが極座標なわけですからHtは直交座標から極座標への変換行列になってるはずですけどそうは見えなかったもので。
その捉え方で間違いありません。
ここでHtは、線形近似するためのヤコビ行列になっています。
なってましたねすいません
お金ないのでScilabに移植して再現を試みてます
Pが発散したりして移植をミスったのか元からそうなのか...
ところでzは2x1行列ですよね?
観測残差yも2x1になるはずですが
推定状態を求めるときにyが転置されていて次元が合わないんですが。
尽力していただきとても感謝しています。
自分の予想でも、Pのような共分散行列やSの逆行列の発散が原因ではないかと考えているのですが、、
いまだにわかりません、、
今確かめたところ、観測値:zおよび、観測の予測値：zpred、観測残差ｙはすべて１×２行列でした。
初期設定で z = [0;0]としているため、2×１行列と自分でも認識していましたが、この設定したｚは使用されず、関数Observation()で設定されているｚ（おそらくデフォルトで１×２行列）が使用されていたため、ｙは転置されていても問題なく計算されいるのだと思います。実際、z ＝[0;0];を削除して実行しても、問題なく実行はされました。
まだまだプログラミングが下手で、誤解を招いてしまい申し訳ありません。よろしくお願いします。
検証中なんですがLMの位置を(0,10)とかに変えると明後日の方向にぶっ飛んでいきません?
確かに、あさっての方向へと飛んでいきます。笑
おそらく、すべて同じ原因が引き起こしているんですよね、、
質問に載せている画像の結果を見ると、ぶっ飛ぶ前は誤差楕円もだんだんと小さくなっていて、しっかり推定されているので、カルマンフィルタ内の更新が完全に原因があると思われます。
回答1件
0
ベストアンサー
質問文の観測のヤコビ行列は整理すると
scilab
1dx = LM(1)-x(1) 2dy = LM(2)-x(2) 3r=(dx^2 + dy^2)^0.5; 4r2=(dx^2 + dy^2); 5Ht = [ 6-dx/r, -dy/r 7-dy/r2, -dx/r2; 8];
となっていますが、
Ht(1,2) = ∂φ/∂xの計算結果だけマイナスが余計で
https://www.wolframalpha.com/input/?i=%28d%2Fdx%29arctan%28%28b-y%29%2F%28a-x%29%29
正しくは
scilab
1dx = LM(1)-x(1) 2dy = LM(2)-x(2) 3r=(dx^2 + dy^2)^0.5; 4r2=(dx^2 + dy^2); 5Ht = [ 6-dx/r, -dy/r 7dy/r2, -dx/r2; 8];
ではないでしょうか
投稿2019/12/04 06:45
編集2019/12/05 08:10総合スコア13551
回答ありがとうございます。
なるほど、そういった事実があるのですか。
自分の載せているコードでは、観測は直交座標へ変換できているでしょうか？
zを(r,θ)じゃなくて
(x,y)にしましょうってことです
観測行列は2x2単位行列
Rは(r,θ)から求めます
とても丁寧なソースコードありがとうございます。
「観測関数はLMから見たロボットの位置になっているのに観測行列はロボットから見たLMの位置になっていたのを修正;(LM-x)に統一」とのことですが、自分の知識が足りず、自分のコードからどの部分が修正されているのかがわかりません、、、
その部分について教えていただけませんか？
あれ?もともとそうでしたね
書き忘れていた変更点追加しました
変更点のご説明、ありがとうございます。
しかし、こちらで実行してみても、同じように軌道が発散するような形になっていしまいました。
共分散行列Pestの固有値がランドマークに近づくにつれ、０に近づいていくのが何かおかしいような気がしています。。。
惑わせてすみません
なんかただ単に観測のヤコビ行列が間違ってる気がします
正しくは
dx = LM(1)-x(1)
dy = LM(2)-x(2)
r2=dx^2 + dy^2;
Ht = [
-dy/(r2)^0.5, dx/(r2)^0.5;
dx/(r2), dy/(r2)];
では?(質問文のコードだとHtの要素全部に-ついている形)
(追記:参考にしたpdfはz(1),z(2)が逆でした)
自分も、Htには多少疑問を感じてはいたのですが、参考にさせていただいているいくつかのウェブページでは自分のソースコードのようになっているので、間違えている可能性は低そうです。
提案していただいた、ヤコビアンで試してみましたが、うまく作動しませんでした。
Pestの固有値（誤差楕円の半径）が小さくなりすぎているのが気になっていて調査中です。
観測のヤコビ行列は
https://tajimarobotics.com/extended-kalman-filter-localization-3/
このサイトを参考にしました。
この形で途中までうまく位置推定ができているのでまちがいないとおもっているのですが、、、
Ht(1,2) = ∂φ/∂xの計算結果だけマイナスが余計です
https://www.wolframalpha.com/input/?i=%28d%2Fdx%29arctan%28%28b-y%29%2F%28a-x%29%29
そのとおりでした！！意図通りにシミュレーションをすることができました！
何日間にもわたって、尽力してくださったozwkさんには本当に感謝しています。
ありがとうございました！
むしろ面倒くさがってちゃんと検算せずに場当たり的になんとかしようとした結果
いろいろ惑わせてすみません
とんでもないです！
最後まで丁寧な対応ありがとうございました！
あなたの回答
tips
プレビュー
