[SOLVED] ANP Homework 2

Theoretical questions:

1. Consider a planning session at time instant 0 and a given set of non-myopic candidate actions A =. {a0:L−1}, where L is the planning horizon. Denote the belief over state Xk at time instant k by bk =. P(Xk | a0:k−1,z1:k), and assume motion and observation models, P(Xk | Xk−1, ak−1) and P(z | X), are given. Consider an objective function J(.,) of expected cumulative rewards given some reward (cost) function r(b,a).

(a) Write the objective function explicitly for the above-considered setting.
(b) Derive a recursive formulation that expresses J(b0,a0:L−1), for a given non-myopic action a0:L−1 ∈ A, via

J(b1,a1:L−1). Present the derivation in detail.
(c) Consider now the corresponding value function V0π(b0) and some stationary policy π within the same

setting.
i. Write an explicit expression for the value function V0π(b0).

ii. Similar to clause 1b, derive a recursive formulation that expresses V0π(b0) in terms of V1π(b1). iii. Discuss the difference between J(b0,a0:L−1) and V0π(b0).

Hands-on tasks: You are highly encouraged to use the Julia language for all hands-on tasks. However, this is not compulsory. In case you use Julia, the supplied skeleton may be helpful.

1. Consider a mobile robot navigating in a 2D environment. For simplicity, we consider only the position state of the robot, i.e. Xi ∈ R2 denotes the x-y robot position at time instant i. The motion and observation models, P(Xk+1 | Xk,ak) and P(zk | Xk), are given by

Xk+1 =f(Xk,ak)+w , zk =h(Xk)+v (1) where w and v are process and measurement noise terms, with w ∼ N(0,Σw) and v ∼ N(0,Σv).

In this exercise, we shall consider simple linear models, i.e. f(Xk,ak) = F ·Xk +ak with F = I2×2 and ak ∈ R2 is the commanded position displacement. We also consider position measurements such that zk ∈ R2 and h(Xk) ≡ Xk.

Consider a prior over robot position at time instant 0 is available: b(X0) =. P(X0) = N(μ0,Σ0).
(a) Implement a function PropagateUpdateBelief, which updates a Gaussian belief according to a (given)

performed action and captured observation, considering a recursive setting.

• Input: a Gaussian belief from some time instant k over Xk, i.e. b(Xk) = N(μk,Σk), action ak and observation zk+1.
• Output: an updated posterior Gaussian belief at the next time step, i.e. b(Xk+1) = N(μk+1,Σk+1). 1

2. (b)  Implement a function SampleMotionModel that, given a robot state X and action a, generates the next state, i.e. X′ ∼ P(X′ | X, a), according to the motion model in Eq. (1).

   • Input: State X, action a

   • Output: Next state X′

3. (c)  Implement a function GenerateObservation, which generates an observation according to the obser-

   vation model in Eq. (1), i.e. z ∼ P(z | X). • Input: State X
   • Output: Observation z

4. (d)  Consider b(X0) = N(μ0, Σ0) with μ0 = (0, 0)T and Σ0 = I2×2, Σw = 0.12 · I2×2, Σv = 0.012 · I2×2, and ak = (1, 0)T . Let the actual initial robot location (ground truth) be X0 = (0.5, 0.2)T .
   1. Generate a possible robot trajectory τ =. {X0, X1, . . . , XT } where the robot starts at X0 and follows a given action sequence a0:T−1 =. {a0,a1,…,aT−1}. Assume T = 10, and ai = (1,0)T for i ∈ [0,T −1].
   2. Generate observations along the trajectory τ, by generating a single zi for each Xi ∈ τ.

iii. Calculate belief propagation along the trajectory τ while only considering a motion model (with-

out observations). In other words, for each time step i ∈ [0, T ], calculate P(Xi | a0:i−1 , b(X0 )) =.

N(μmm,Σmm), considering the prior belief b(x ) and the probabilistic motion model (mm). Draw on ii0

a plot the actual trajectory and these propagated beliefs1 (in terms of mean and covariance) versus

time.
iv. Calculate the posterior beliefs along the trajectory τ, this time also using observations (generated ob-

servations in 1(d)ii). In other words, for each time step i ∈ [0, T ], calculate P(Xi | a0:i−1 , z1:i , b(X0 )) =. N(μi,Σi), considering the prior belief b(X0) and the probabilistic motion and observation models. Draw on a separate plot the actual trajectory, and these posterior beliefs (in terms of mean and covariance) versus time.

2. We now modify the problem setting as follows. Several beacons are scatted in the environment. The locations of these beacons are known to the robot (denote it by Xjb for the jth beacon), and represented by a matrix Xb ∈ R2×n, i.e. Xb = [X1b,…,Xnb], where n is the number of beacons.

Assume the robot gets a relative position observation zrel ∈ R2 from a beacon if it is sufficiently close to it (distance less than d). For simplicity, consider the beacons are positioned sufficiently apart such that an observation from only one of the beacons can be obtained for every possible robot location. Moreover, assume the accuracy of these observations deteriorates with the distance r to the beacon, starting from a certain given distance rmin ≤ d, such that for r ≤ d, the measurement covariance is Σv = (0.01 · max(r, rmin ))2 · I2×2 .

1. (a)  Write the corresponding observation model for the measurement zrel, as a function of robot location X, beacon location Xb, and additional parameters mentioned above.
2. (b)  Implement a function GenerateObservationFromBeacons, which generates an observation zrel ac- cording to the observation model from clause 2a. Assume Xb,r,rmin and d are given.

   • Input: Robot location X

   • Output: Measurement zrel and index of beacon within range; null otherwise.

3. (c)  Consider n = 9 beacons equally scattered in a 9×9 grid, and let the coordinate of its bottom left corner be (0, 0). To clarify, one of the beacons is located at (0, 0). Assume b(X0) = N(μ0, Σ0) with μ0 = (0, 0)T and Σ0 = I2×2, Σw = 0.12 · I2×2. Let the actual initial robot location (ground truth) be X0 = (−0.5, −0.2)T . Consider an action sequence a0:T−1 = {a0,…,aT−1}, with ai =. (0.1,0.1)T, i ∈ [0,T −1], for T = 100. Further, let d = 1 and rmin = 0.1.
   1. Repeat clauses 1(d)i-1(d)iv. Indicate in all plots also the n beacons.
   2. Repeat clauses 1(d)i-1(d)iv, while considering a fixed measurement covariance Σv = 0.012I2×2 (mea-

      surement is still obtained only if r ≤ d). Indicate in all plots also the n beacons.

   3. Present in a new figure localization estimation errors2 X ̃ versus time for the two cases above.
   4. Present in a new figure the square root of the trace of estimation covariance for the two cases above.

      Compare and analyze qualitatively the obtained results.

1You can use the supplied drawCovarianceEllipse function to draw a 1-std covariance ellipse. Alternatively, similar Julia function covellipse! from package StatsPlots.jl is available.

2X ̃ = Xˆ − X, where Xˆ is an estimate and X is the ground truth value. 2

(d) Leveraging the above capabilities, let us now consider a simple belief space planning problem. Given a set of action sequences A = {aj0:T−1}, the robot has to choose the one that is expected to yield minimum uncertainty at the final state (XT ). In this question we consider det(Σ) as the information-theoretic cost function c(b, a).

Further, consider N = 10 simple action sequences in A = {aj0:T−1}Nj=1, where the jth action sequence aj0:T−1 isgivenbyaj0:T−1 ={aj0,…,ajT−1},whereaji =. (0.1,0.1·j/5)T.

1. Generate a single trajectory realization τj for each action sequence in A = {aj0:T−1}Nj=1 by following clause 1(d)i. Draw all N trajectories and indicate beacon locations on a single plot.
2. Calculate the posterior beliefs along each trajectory τj, ∀j ∈ [1,N], and evaluate the information theoretic cost. Plot the information-theoretic cost versus trajectory index. Designate the best action for the given cost, and explain why this result makes sense.

3