Download as PDF, PPTX
More Related Content
Similar to OutLookAR for EKF_SLAM_Model
OutLookAR for EKF_SLAM_Model
- 1.
- 2. 2 もくじ 1. はじめに 2. 事前条件 3.モデル 4. おわりに
- 3.
- 4. 1. はじめに Simultaneous Localizationand Mapping (SLAM) 4 ・自己位置推定と環境地図作成を同時に行う ・鶏が先か、卵が先か
- 5. 2. 事前条件 SLAMに関する定義 5 Given Wanted ・入力値 ・観測値 ・環境地図 ・自己姿勢 z1:T ={z1, z2, z3..., zT } u1:T = {u1, u2, u3..., uT } m1:n = {m1, m1, m2, ..., mn} xT
- 6. 2. 事前条件 入力値 6 直近のトラッキング結果の姿勢 クォータニオン u =xT 1 = (qx, qy, qz, qw)T
- 7.
- 8.
- 9. 2. 事前条件 移動軌跡 9 カメラの自己姿勢 クォータニオン x =(qx, qy, qz, qw)T
- 10. 3. モデル アルゴリズム 10 Extended Kalmanfilter(µt 1, t 1, ut, zt) µt = g(ut, µt 1) Kt = tHT t (Ht tHT t + Qt) 1 µt = µt + Kt(zt h(µt)) t = (I KtHt) t return µt, t t = Gt t 1GT t + Rt
- 11. 3. モデル 状態空間 11 EKF SLAMの状態空間 xt= (qx, qy, qz, qw Camera s OR , m1,x, m1,y, m1,z landmark 1 , ..., mn,x, mn,y, mn,z landmark n )T
- 12.
- 13.
- 14. 3. モデル Prediction Step 14 カメラ自己姿勢の予測モデル 回転Quaternionの合成は掛け算によって表現する 実際は q= q q + qt = q + q + qt q = q q qt = qt
- 15.
- 16. 3. モデル Prediction Step 16 状態行列の予測モデル g(ut,µt 1) = µt 1 + FT x µt 1,qx + ut,qx µt 1,qy + ut,qy µt 1,qz + ut,qz µt 1,qw + ut,qw = FT x ut = qx qy qz qw ... + 1 0 0 0 0 . . . 0 0 1 0 0 0 . . . 0 0 0 1 0 0 . . . 0 0 0 0 1 0 . . . 0 T qxt qx qyt qy qzt qz qwt qw FT x =
- 17. 3. モデル Prediction Step 17 共分散行列の更新 Gt= Gx t 0 0 I ヤコビ行列(4x4) 単位行列(3Nx3N)
- 18. 3. モデル Prediction Step 18 共分散行列の更新 ヤコビ行列 Gx t= (qx, qy, qz, qw)T qx + qxt qx qy + qyt qy qz + qzt qz qw + qwt qw = 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
- 19. 3. モデル Prediction Step 19 Fx= 1 0 0 0 0 . . . 0 0 1 0 0 0 . . . 0 0 0 1 0 0 . . . 0 0 0 0 1 0 . . . 0 1: 2: µt = µt 1 + FT x qxt qx qyt qy qzt qz qwt qw µt 1,qx + ut,qx µt 1,qy + ut,qy µt 1,qz + ut,qz µt 1,qw + ut,qw EKF SLAM Prediction(µt 1, t 1, ut, zt, ct, Rt)Rx t )
- 20. 3. モデル Prediction Step 20 Gt= 0 0 0 I 以上 t = Gt t 1GT t + FT x Rx t Fx Rt 3: 4:
- 21. 3. モデル Correction Step 21 1 実際のランドマークの位置 カメラからランドマークまで1進んだ位置 特徴点の位置 画面左上からの特徴点位置から 単位球面上に投影された座標への変換 画面のセンターまでの座標:c = {cx, cy}Tc f 焦点距離(ピクセル): f zi t = xi t yi t 特徴点の位置:
- 22. 3. モデル Correction Step 22 即知のデータの関連付け ・特徴点検知zi t = xi t yi t i番目の測定は、j番目のランドマークを観測j = ci t・
- 23. 3. モデル Correction Step 23 画面左上からの特徴点位置から中心からの座標に変換 焦点距離よりスクリーンまでは 単位球面上に投影された座標は よって µj,x µj,y µj,z = xi t y i t f x i t y i t = xi t cx yi t cy µj,x µj,y µj,z = 1 µj,x 2 + µj,y 2 + µj,z 2 µj,x µj,y µj,z = 1 (xi t cx) 2 + (yi t cy) 2 + f2 xi t cx yi t cy f
- 24. 3. モデル Correction Step 24 ・始めてランドマークを観測した場合 xi tcx f2+(xi t cx)2+(yi t cy)2 yi t cy f2+(xi t cx)2+(yi t cy)2 f f2+(xi t cx)2+(yi t cy)2 µj,x µj,y µj,z = µ2 t,qw + µ2 t,qx µ2 t,qy µ2 t,qz 2(µt,qx µt,qy µt,qw µt,qz ) 2(µt,qx µt,qz + µt,qw µt,qy ) 2(µt,qx µt,qy + µt,qw µt,qz ) µ2 t,qw µ2 t,qx + µ2 t,qy µ2 t,qz 2(µt,qy µt,qz µt,qw µt,qx ) 2(µt,qx µt,qz µt,qw µt,qy ) 2(µt,qy µt,qz + µt,qw µt,qx ) µ2 t,qw µ2 t,qx µ2 t,qy + µ2 t,qz µj,x µj,y µj,z = µt,q µj,x µj,y µj,z µt,q クォータニオンを回転行列で表す
- 25. 3. モデル Correction Step 25 h(µt)= 1 2qwqyµx + 2qxqzµx 2qwqxµy + 2qyqzµy + µz 2q2 xµz 2q2 yµz cx(2qwqyµx + 2qxqzµx 2qwqxµy + 2qyqzµy + µz 2q2 xµz 2q2 yµz)+ f(µx 2q2 yµx 2q2 zµx + 2qxqyµy + 2qwqzµy 2qwqyµz + 2qxqzµz) cy(2qwqyµx + 2qxqzµx 2qwqxµy + 2qyqzµy + µz 2q2 xµz 2q2 yµz)+ f( 2qwqzµx + µy 2q2 xµy 2q2 zµy + 2qyqzµz + 2qx(qyµx + qwµz)) ランドマークの観測モデル f 0 cx 0 f cy 0 0 1 1 2µ2 t,qy 2µ2 t,qz 2µt,qx µt,qy + 2µt,qw µt,qz 2µt,qx µt,qz 2µt,qw µt,qy 2µt,qx µt,qy 2µt,qw µt,qz 1 2µ2 t,qx 2µ2 t,qz 2µt,qy µt,qz + 2µt,qw µt,qx 2µt,qx µt,qz + 2µt,qw µt,qy 2µt,qy µt,qz 2µt,qw µt,qx 1 2µ2 t,qx 2µ2 t,qy
- 26. 3. モデル Correction Step 26 ランドマークの観測モデル f0 cx 0 f cy 0 0 1 1 2µ2 t,qy 2µ2 t,qz 2µt,qx µt,qy + 2µt,qw µt,qz 2µt,qx µt,qz 2µt,qw µt,qy 2µt,qx µt,qy 2µt,qw µt,qz 1 2µ2 t,qx 2µ2 t,qz 2µt,qy µt,qz + 2µt,qw µt,qx 2µt,qx µt,qz + 2µt,qw µt,qy 2µt,qy µt,qz 2µt,qw µt,qx 1 2µ2 t,qx 2µ2 t,qy µj,x µj,y µj,z s ˆxi t ˆyi t 1 = f 0 cx 0 f cy 0 0 1 µt,q µj,x µj,y µj,z µt,q = ˆzi t = ˆxi t ˆyi t = 1 (2µt,qw µt,qy + 2µt,qx µt,qz )µj,x + ( 2µt,qw µt,qx + 2µt,qy µt,qz )µj,y + (1 2µ2 t,qx 2µ2 t,qy )µj,z (cx(2µt,pw µt,py + 2µt,px µt,pz ) + f(1 2µ2 t,py 2µ2 t,pz ))µj,x+ (f(2µt,px µt,py + 2µt,pw µt,pz ) + cx( 2µt,pw µt,px + 2µt,py µt,pz ))µj,y+ (cx(1 2µ2 t,px 2µ2 t,py ) + f( 2µt,pw µt,py + 2µt,px µt,pz ))µj,z (f(2µt,qx µt,qy 2µt,qw µt,qz ) + cy(2µt,qw µt,qy + 2µt,qx µt,qz ))µj,x+ (cy( 2µt,qw µt,qx + 2µt,qy µt,qz ) + f(1 2µ2 t,qx 2µ2 t,qz ))µj,y+ (cy(1 2µ2 t,qx 2µ2 t,qy ) + f(2µt,qw µt,qx + 2µt,qy µt,qz ))µj,z
- 27. 3. モデル Correction Step 27 B= 2qwqyµx + 2qxqzµx 2qwqxµy + 2qyqzµy + µz 2q2 xµz 2q2 yµz low Hi t qx = 2f(2q3 zµ2 x 2q2 wqyµyµz + (qx(2 4q2 y)µx + qyµy + 2q2 xqyµy 2q3 yµy)µz+ q2 z( 4qwµxµy 4qxµxµz + 2qyµyµz) + qw(µxµy 4qxqyµ2 z)+ qz(( 1 + 2q2 y)µ2 x + 2q2 wµ2 y + 2q2 yµ2 y + 4qwqyµxµz + 4qwqxµyµz + µ2 z + 2q2 xµ2 z 2q2 yµ2 z)) B2 2f(q2 w( 2qzµxµy + 2qyµxµz) + (qzµy qyµz)(( 1 2q2 x + 2q2 y + 2q2 z)µx 4qx(qyµy + qzµz))+ qw(2q2 z(µ2 x µ2 y) 4qxqzµxµz + qy( 4qxµxµy + 4qzµyµz) + 2q2 y(µ2 x µ2 z) + (1 + 2q2 x)(µ2 y + µ2 z))) B2 low Hi t qy = 2f(2q2 wµy(qzµx qxµz)+ ( qzµx + qxµz)( 4qxqyµx + 2q2 xµy + ( 1 2q2 y + 2q2 z)µy 4qyqzµz) + qw((1 + 2q2 y 2q2 z)µ2 x+ 2q2 zµ2 y 4qyqzµyµz + µ2 z + 2q2 yµ2 z + qxµx( 4qyµy + 4qzµz) + 2q2 x(µ2 y µ2 z))) B2 2f( qzµ2 y + 2q3 zµ2 y 2q3 xµxµz + qx(1 + 2q2 y + 2q2 z)µxµz + 2qyµyµz 4qyq2 zµyµz+ qzµ2 z + 2q2 yqzµ2 z + 2q2 wµx(qzµx qxµz)+ qw(4qxµz( qzµy + qyµz) + µx( µy + 4q2 zµy 4qyqzµz)) + 2q2 x( 2qyµyµz + qz(µ2 x + µ2 y µ2 z))) B2 観測モデルのヤコビ行列
- 28. 3. モデル Correction Step 28 low Hi tqz = 2f( 2q2 wqyµxµy + 2q3 xµ2 z + µx(qyµy 2q3 yµy + 2qyq2 zµy + 2qzµz 4q2 yqzµz)+ qw(4qyqzµ2 x µyµz) + 2q2 x(qyµxµy 2qzµxµz + 2qwµyµz)+ qx((1 2q2 y + 2q2 z)µ2 x + 2q2 wµ2 y + 2q2 yµ2 y µ2 z + 2q2 yµ2 z 4qwµx(qzµy + qyµz))) B2 2f( 2q3 xµxµy + qx(1 + 2q2 y + 2q2 z)µxµy + qyµ2 y + 2qyq2 zµ2 y + 2q2 wµx(qyµx qxµy)+ 2qzµyµz 4q2 yqzµyµz qyµ2 z + 2q3 yµ2 z+ qw( 4qxqzµ2 y + µxµz 4q2 yµxµz + 4qyµy(qzµx + qxµz)) + 2q2 x( 2qzµyµz + qy(µ2 x µ2 y + µ2 z))) B2 low Hi t qw = 2f(µy(qxµx + qzµz) 4q2 yµy(qxµx + qzµz) + 2q3 y(µ2 x + µ2 z)+ qy(( 1 + 2q2 z)µ2 x + 2q2 zµ2 y 4qxqzµxµz µ2 z + 2q2 x(µ2 y + µ2 z))) B2 2f(µx(qyµy + qzµz) 4q2 xµx(qyµy + qzµz) + 2q3 x(µ2 y + µ2 z)+ qx( µ2 y + 2q2 z(µ2 x + µ2 y) 4qyqzµyµz µ2 z + 2q2 y(µ2 x + µ2 z))) B2
- 29. 3. モデル Correction Step 29 low Hi tµx = f( 2qwqxµy + 4q2 wqy( qzµy + qyµz) + ( 1 + 2q2 x + 2q2 y + 2q2 z)( 2qyqzµy µz + 2q2 yµz)) B2 2f(qx( 1 + 2q2 x + 2q2 y + 2q2 z)( qzµy + qyµz) + q2 wqx( 2qzµy + 2qyµz) + qw(qyµy + qzµz)) B2 low Hi t µy = 2f(2q2 wqy(qzµx qxµz) + qy( 1 + 2q2 x + 2q2 y + 2q2 z)(qzµx qxµz) + qw(qxµx + qzµz)) B2 f(2qwqyµx + 4q2 wqx( qzµx + qxµz) + ( 1 + 2q2 x + 2q2 y + 2q2 z)( 2qxqzµx µz + 2q2 xµz)) B2 low Hi t µz = f((1 + 4q4 y + q2 x( 2 + 4q2 y) 2q2 z + 4q2 y( 1 + q2 w + q2 z))µx+ 2( 2q3 xqy + qwqz qxqy( 1 + 2q2 w + 2q2 y + 2q2 z))µy) B2 f(4q3 xqyµx + 2qwqzµx + 2qxqy( 1 + 2q2 w + 2q2 y + 2q2 z)µx 4q4 xµy 4q2 x( 1 + q2 w + q2 y + q2 z)µy + ( 1 + 2q2 y + 2q2 z)µy) B2
- 30. 3. モデル Correction Step 30 10 0 0 0 . . . 0 0 0 . . . 0 0 1 0 0 0 . . . 0 0 0 . . . 0 0 0 1 0 0 . . . 0 0 0 . . . 0 0 0 0 1 0 . . . 0 0 0 . . . 0 0 0 0 0 0 . . . 1 0 0 . . . 0 0 0 0 0 0 . . . 0 1 0 . . . 0 0 0 0 0 0 . . . 0 0 1 . . . 0 Hi t = low Hi t Fx,j =
- 31. 3. モデル Correction Step 31 Qt= 2 x 0 0 2 y for all observed features zi t = (xi t, yi t)T do j = ci t if landmark j never seen before xi t cx f2+(xi t cx)2+(yi t cy)2 yi t cy f2+(xi t cx)2+(yi t cy)2 f f2+(xi t cx)2+(yi t cy)2 µj,x µj,y µj,z = µ2 t,qw + µ2 t,qx µ2 t,qy µ2 t,qz 2(µt,qx µt,qy µt,qw µt,qz ) 2(µt,qx µt,qz + µt,qw µt,qy ) 2(µt,qx µt,qy + µt,qw µt,qz ) µ2 t,qw µ2 t,qx + µ2 t,qy µ2 t,qz 2(µt,qy µt,qz µt,qw µt,qx ) 2(µt,qx µt,qz µt,qw µt,qy ) 2(µt,qy µt,qz + µt,qw µt,qx ) µ2 t,qw µ2 t,qx µ2 t,qy + µ2 t,qz endif 6: 5: 7: EKF SLAM CorrectionEKF SLAM Prediction(µt 1, t 1, ut, zt, ct, Rt)Rx t )
- 32. 3. モデル Correction Step 32 zi t= cx(2µt,qw µt,qy µj,x + 2µt,qx µt,qz µj,x 2µt,qw µt,qx µj,y+ 2µt,qy µt,qz µj,y + µj,z 2µ2 t,qx µj,z 2µ2 t,qy µj,z)+ f(µj,x 2µ2 t,qy µj,x 2µ2 t,qz µj,x + 2µt,qx µt,qy µj,y+ 2µt,qw µt,qz µj,y 2µt,qw µt,qy µj,z + 2µt,qx µt,qz µj,z) 2µt,qw µt,qy µj,x+2µt,qx µt,qz µj,x 2µt,qw µt,qx µj,y+2µt,qy µt,qz µj,y+µj,z 2µ2 t,qx µj,z 2µ2 t,qy µj,z cy(2µt,qw µt,qy µj,x + 2µt,qx µt,qz µj,x 2µt,qw µt,qx µj,y+ 2µt,qy µt,qz µj,y + µj,z 2µ2 t,qx µj,z 2µ2 t,qy µj,z)+ f( 2µt,qw µt,qz µj,x + µj,y 2µ2 t,qx µj,y 2µ2 t,qz µj,y+ 2µt,qy µt,qz µj,z + 2µt,qx (µt,qy µj,x + µt,qw µj,z) 2µt,qw µt,qy µj,x+2µt,qx µt,qz µj,x 2µt,qw µt,qx µj,y+2µt,qy µt,qz µj,y+µj,z 2µ2 t,qx µj,z 2µ2 t,qy µj,z Ki t = tHi t T (Hi t tHi t T + Qt) 1 µt = µt + Ki t(zi t zi t) = µt,q Ki t,q(zi t zi t) ... µt,v + Ki t,v(zi t zi t) ... 8: 9: 10:
- 33. 3. モデル Correction Step 33 t= (I Ki tHi t ) t end for t = t µt = µt return µt, t 以上 11: 12: 13: 14:
- 34. 4. おわりに 34 Prediction Step＆CorrectionStep を繰り返すことでEKF SLAMを実現させる 問題点または、不足な点がございましたら ご教授願います。