yair ben elisha - github pages...(bsp) for visual-inertial navigation systems allows to consider,...
TRANSCRIPT
![Page 1: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/1.jpg)
Graduate Seminar, July 2017
Yair Ben Elisha
Under the supervision of Asst. Prof. Vadim Indelman
![Page 2: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/2.jpg)
•Introduction
•Related work
•Contribution
•Single Robot Approach
•Multi-robot Approach
•Conclusion
![Page 3: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/3.jpg)
Autonomous cars(google)
Military Mines/Tunnels
Exploration
Space Exploration
Sub-marine
explorationIndoor Operation
![Page 4: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/4.jpg)
•Navigation and mapping in unknown, GPS-deprived,
environment
•Key components for autonomous operation include:
▪ Estimation and Perception – Where am I? What is the environment?
• Simultaneous localization and mapping (SLAM)
▪ Planning – What is the best action to do next?
• Traditional planning approaches
• Belief Space Planning (BSP)
Perception
Planning
Estimation‘Passive’
‘Active’
![Page 5: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/5.jpg)
Inertial sensor
offline calibration
Camera
offline calibration
Inertial Sensor
Online calibration (pre-determined maneuvers)
![Page 6: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/6.jpg)
Sensors
Calibration
Goal
“GPS”
“Sensors
Calibration”
![Page 7: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/7.jpg)
• Aerial cooperative multi-robot
• Sensors:
▪ Inertial Measurements Unit (IMU)
▪ Monocular downward-looking camera
• Vision inertial navigation system (VINS)
• Partially unknown environment
• GPS-deprived environment
• Centralized architecture
Planning optimal trajectories for cooperative
robots to achieve online IMU calibration and
accurate navigation
![Page 8: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/8.jpg)
• Online Calibration
(V. Indelamn, 2012), (J. Maye, 2016)
▪ SLAM considering IMU and extrinsic parameters calibration
▪ Without planning
• Belief Space Planning (BSP)
(V. Indelman, 2013), (G. A. Hollinger, 2014) , (V. Indelman, 2015)
▪ Performance improvement in SLAM
▪ Not considering IMU measurements
• Planning Considering Online Calibration
▪ Extrinsic parameters calibration (transformation between frames)
(W. Achtelik, 2013), (D.J. Webb, 2014), (J. Maye, 2016)
▪ IMU calibration assuming GPS availability
(K. Hausman, 2016)
![Page 9: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/9.jpg)
• Multi-robot Belief Space Planning
(A. Kim, 2014, IJRR) (V. Indelman, 2015, IJRR) (V. Indelman, 2015, ISRR)
▪ Operation in unknown environments
▪ Cooperation via mutual landmark observations or observing other robots
(V. Indelman, 2015, ISRR)
![Page 10: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/10.jpg)
• Incorporating online sensor calibration into belief space planning
(BSP) for visual-inertial navigation systems
▪ Allows to consider, within planning, reduction of navigation estimation
uncertainty and reduction of the uncertainty evolution rate
• Approach for cooperative multi-robot BSP using future indirect
constraints given past correlation between robots
▪ Introduce concept of “expendable” robots used for updating other robots
• Incorporate recently developed concept of IMU pre-integration into BSP
![Page 11: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/11.jpg)
Robot’s Nav.
State
0{ , , }r r r
k kX x x
IMU Sensors
Calibration
0{ , , }r r r
k kC c c
Perceived
Environment
0{ , , }nkL l l
Joint State
{ , , }k k k kX C L
Control
Actions
0: 1 0 1{ , , }r r r
k kU u u
Sensors
Measurements
1: 1{ , , }r r r
k kZ z z
1
Rr
k k rX X
1
Rr
k k rC C
Random
Varia
ble
sG
iven
![Page 12: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/12.jpg)
![Page 13: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/13.jpg)
• Joint probability distribution function (pdf)
1: 0: 1
1 1 1 1
1
| ,
· | , , · | |
k k k k
kIMU o
i i i i i i i i
i
b p Z U
priors p x x c z p c c p z
Motion ModelObservation
ModelCalibration
Model
![Page 14: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/14.jpg)
1 1 1| , , IMU
i i i ip x x c z
Motion Model:
Strapdown Equations:
1 1 1, ,
1
2
i i
IMU T m a
i i i i i i i
m g
i i i i
p v
f x c z v C q a b n g
q d n q
1 1 1 1
1
[ ]
, ,
: ~ 0,
i i i i
IMU
i i i i i
i w
x p v q
x f x c z w
Gaussian Noise w N
![Page 15: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/15.jpg)
Calibration Model:
Random constant model:
1|i ip c c
1 1i i ic c e
1 1
1
[ ]
: ~ 0,
i i i
i i i
i e
c d b
c g c e
Gaussian Noise e N
![Page 16: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/16.jpg)
Single Observation Model:
General Observation Model:
| o
i ip z
, ,
: ~ 0,
,
i j i j i
i v
i j
z h x l v
Gaussian Noise v N
h x l Pinhole Camera Model
,| | ,
:
oj k
o
k k k j k j
l
o
k k
p z p z x l
Landmarks observed from xk
![Page 17: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/17.jpg)
1 1 1 1| , , | |IMU o
i i i i i i i ip x x c z p c c p z
Motion ModelObservation
Model
Calibration
Model
1 1 1 1
1
[ ]
, ,
~ 0,
i i i i
IMU
i i i i i
i w
x p v q
x f x c z w
w N
1 1 1 1
1
[ ]
~ 0,
i i i
i i i i i
i e
c d b
c g c e c e
e N
,
,
,
~ 0,
| | ,o
j k
i j i j i
i v
o
k k k j k j
l
o
k k
z h x l v
v N
p z p z x l
![Page 18: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/18.jpg)
• Factorization of a joint pdf in terms
of process and measurement models
▪ Vertices represent the variables
▪ Nodes represent constrains between
variables, the factors
• Computationally efficient
probabilistic inference
![Page 19: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/19.jpg)
• The belief at the lth look ahead time is defined as:
•Maximum a posteriori (MAP) inference:
0: 0: 1
1 1 1 1 1| , ,
|
|
,
|
k l k l k l k l
o
k l k l k l k l k l k l k l k l k l
b p Z
b p x x u c p c c p z
U
,k l k l k lb
![Page 20: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/20.jpg)
•Challenge:
▪ High rate IMU measurements
▪ Updating the graph at high rate
▪ High complexity
▪ No real time performance in inference, limits planning horizon in BSP
![Page 21: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/21.jpg)
•Solution:
▪ Integrate IMU measurements into a single factor
▪ Add corresponding factor at the frequency of other sensors (e.g.
camera)
▪ Previous works use this concept within inference only
▪ Additionally use this concept within BSP to increase planning horizon
[T. Lupton, 2012]
[V. Indelman, 2013]
![Page 22: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/22.jpg)
• The objective function over a planning horizon of L steps:
•Optimal control:
• Choice of cost function
1
: 1
0
, ,L
k k L k k L l k l k l L k L
l
J b U cf b u cf b
: 1
: 1 : 1argmin ,k k L
kU
k L k k L k k LU J b U
penalizes joint state uncertaintypenalizes control actionspenalizes reaching the goal
,u
Goal T
k l k l k l k l k lMMcf b u X X u tr M M
, k lcf M
Cost function
![Page 23: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/23.jpg)
•Optimal control:
• Discrete Methods:
▪ Choose best path from a set of candidate paths
▪ Sampling methods (e.g. RRT or PRM)
• Continuous Methods:
▪ Direct optimization or Gradient descent methods
: 1
: 1 : 1argmin ,k k L
kU
k L k k L k k LU J b U
![Page 24: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/24.jpg)
•Matlab simulation using GTSAM library
• Synthetic IMU measurements and camera observations
• Assuming data association is solved
• Assuming heading angle control only
• A priori-known regions with different uncertainty levels
![Page 25: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/25.jpg)
• Theorem: Full observability requires the camera-IMU
platform to undergo rotation about at least two IMU axes
and acceleration along two IMU axes
• Conclusion: Heading angle control is not sufficient for full
IMU calibration
• Alternatives: Using a priori known regions with different
levels of uncertainty to calibrate accelerometers only
[Achtelik13icra]
![Page 26: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/26.jpg)
![Page 27: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/27.jpg)
Position Covariance Accel. Calib. Covariance
Known Region
Known Region
![Page 28: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/28.jpg)
• Environment
▪ Randomly scattered unknown landmarks
▪ Goal location within area without landmarks at all (“dark corridor”)
▪ A priori-known regions with different uncertainty levels
• Discrete method – generate candidate paths
▪ Shortest path to goal
▪ Shortest paths to clusters of mapped/known landmarks
![Page 29: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/29.jpg)
• Approach 1 – ‘BSP-Calib’ (our approach)
▪ Incorporate sensor calibration states within the belief
▪ “Trade-off” uncertainty cost function
• Approach 2 – ‘BSP’
▪ Does not incorporate sensor calibration states within the belief
• Approach 3 – ‘Shortest-Path’
▪ Uncertainty cost function set to zero
TO
c c x x
T T
k l k lcf tr M M tr M M
![Page 30: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/30.jpg)
BSP-Calib BSP Shortest-Path
c c
u
G G T
k l k l k l k lMMlcf E u tr M M
Uncertainty = 10m Uncertainty = 1e-5m - Covariance
![Page 31: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/31.jpg)
Accel. Calibration CovarianceKnown Region
![Page 32: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/32.jpg)
28.5
21.1
Position Covariance“Dark Corridor”
![Page 33: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/33.jpg)
•Online active self-calibration of IMU in GPS-deprived
environment using BSP
• Improved navigation accuracy
• Incorporated IMU pre-integration concept within BSP for
longer planning horizons
![Page 34: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/34.jpg)
![Page 35: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/35.jpg)
• Cooperative multi-robot:
▪ Robust and faster exploration/mapping
▪ Higher accuracy in a multi robot collaborative framework
Cooperation via mutual
landmark observationsCooperation via
observing other robots
![Page 36: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/36.jpg)
Cooperation via
observing same landmark
Cooperation via
observing other robots
Indirect cooperation
given past correlation
Given correlation,
all robots are updated when a
single robot observes a (known)
landmark
![Page 37: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/37.jpg)
• Relaxing previous cooperation constraints
• Requires initial/past correlation between robots
•Given prior correlation, informative observation made by one
robot, impacts also the states of other robots
Note: Study of correlation decay with time or sensitivity to correlation
magnitude – not part of this work
Given correlation,
all robots are updated when a single robot
observes a (known) landmark
![Page 38: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/38.jpg)
• Study case:
▪ Two robots, r1 and r2 , starting with some initial correlation
▪ r1 observes a prioiri known landmark
▪ r2 does not make any observations
• Definition of covariance matrix Σ or the information matrix I
• R - The square root information matrix
11 121
21 22
,TI R R I
![Page 39: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/39.jpg)
• r1 observes the known region at tk
2
1
1 1
1 1
. # .
0
,
X n
rJacob n Robots o
Tr r
erv
r
bs
A h a
X x x z
a a
h x v
11 12
21 22
k k
k
k k
![Page 40: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/40.jpg)
• Calculation of Σ22
2
2 11
22 22 2 2 21 1
22 11 1 12
k
k
k k k
rr
r r a r
11 12
21 22
k k
k
k k
![Page 41: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/41.jpg)
•Examined Trajectories:
“Shortest-Path” “Future-Correlation”“Higher-Correlation”
Low Prior Correlation High Prior Correlation No Prior Correlation
![Page 42: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/42.jpg)
•Position Accuracy (errors and covariance):
“Shortest-Path” “Future-Correlation”“Higher-Correlation”
![Page 43: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/43.jpg)
• The belief of R robots is defined as:
• Extendable to BSP for the lth look ahead time
, , . ,
1 1 1 1
1 1
| , , | |R k
r r r r r IMU r r r cam r o
k i i i i i i i i
r i
b priors p x x c z p c c p Z
![Page 44: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/44.jpg)
•General objective function
• Cost function
1
: 1
0
1
, ,L
k k L k k L l k l k l L k L
l
Ri
i
J b U cf b u cf b
cf cf
1
,r r
ru
Ri Goal r r
k l k l k lM Mr
cf u X X cf M
![Page 45: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/45.jpg)
• Simulation – same as single robot
• Partial unknown environment
▪ Mostly unknown environment
▪ A priori-known regions with different uncertainty levels
• Discrete method – PRM
• Re-planning every 8 steps
• Initial Correlation is created by observing same landmark
![Page 46: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/46.jpg)
1 1
1
11 2 2 1,,G
l k l l k lM
cf X c cf MX f
Uncertainty = 1e-5m - Covariance
![Page 47: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/47.jpg)
Accel. Calibration CovariancePosition Covariance & Error
1 1 1
1
1 2 2 1, ,G G
l k l k l l k lM
cf E cf cf M
![Page 48: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/48.jpg)
•Online self-calibration of IMU in GPS-deprived
environment using BSP for improving navigation accuracy
•Cooperative multi-robot using indirect updates within BSP
![Page 49: Yair Ben Elisha - GitHub Pages...(BSP) for visual-inertial navigation systems Allows to consider, within planning, reduction of navigation estimation uncertainty and reduction of the](https://reader035.vdocuments.mx/reader035/viewer/2022081613/5fb5e87a87e32a534d7dc4a9/html5/thumbnails/49.jpg)