On the Use of the Observability Gramian for Partially Observed Robotic Path Planning Problems
Mohammadhussein Rafieisakhaei, Suman Chakravorty, P. R. Kumar
OOn the Use of the Observability Gramianfor Partially Observed Robotic Path Planning Problems ∗ Mohammadhussein Rafieisakhaei , Suman Chakravorty and P. R. Kumar Abstract — Optimizing measures of the observability Gramianas a surrogate for the estimation performance may provideirrelevant or misleading trajectories for planning under obser-vation uncertainty.
I. I
NTRODUCTION
The Observability Gramian (OG) is used to determinethe observability of a deterministic linear time-varying sys-tem [1]–[3]. For such systems, the properties of the OGhave been well-studied [1], [4], [5]. When sensors providenoisy stochastic measurements, the state is only partiallyobserved. The general problem of planning under processand observation uncertainties has been formulated as sucha stochastic control problem with noisy observations. Thesolution of this problem provides an optimal policy viathe Hamilton-Jacobi-Bellman equation [6], [7]. However,the computational hurdle for finding a solution to theseequations has necessitated the study of a variety of methodsto approximate the solution [8]–[11]. One approach has beento maximize the estimation performance by planning fortrajectories that can exploit the properties of observation,process and a priori models. We examine the appropriatenessor lack thereof of methods based on the OG, and show thatthey can provide misleading trajectories.Borrowed from deterministic control theory, the OG hasbeen exploited in order to provide more observable trajecto-ries, particularly in trajectory planing problems [12]–[18].In the special case of a diagonal observation covariancewith the same uncertainty level in each direction [1], theStandard Fisher Information Matrix (SFIM) does reduce tothe OG. Indeed the usage of the OG in filtering problemshas been justified through its connections to the SFIM andits relations to the parameter estimation problem [13], [19].In fact, tailored to the parameter estimation problem, theSFIM only addresses the amount of information in the mea-surements alone [1], and neglects both the prior informationand process uncertainty. Closely-related approaches are themethods that base their planning on the observation modelor the likelihood function [8], [20], and the analysis of thispaper can be helpful in providing a better understanding ofthose problems. *This material is based upon work partially supported by NSF underContract Nos. CNS-1646449 and Science & Technology Center Grant CCF-0939370, the U.S. Army Research Office under Contract No. W911NF-15-1-0279, and NPRP grant NPRP 8-1531-2-651 from the Qatar NationalResearch Fund, a member of Qatar Foundation. M. Rafieisakhaei and P. R. Kumar are with the Department of Electricaland Computer Engineering, and S. Chakravorty is with the Departmentof Aerospace Engineering, Texas A&M University, College Station, Texas,77840 USA. { mrafieis, schakrav, [email protected] } In contrast, the
Posterior
FIM (PFIM), whose inversecoincides with the Posterior Cram´er-Rao Lower Bound forthe estimation uncertainty in a general stochastic problem[21], can capture the history of evolution of uncertainty inthe problem. In particular, for a linear system, it has beenshown that the Riccati equations for the covariance evolutionof the state estimation resulting from the Kalman Filter (KF)coincide with evolution of the PFIM in the form of theinverse covariance or the information filter [21]–[24]. Indeed,it is only this measure that can capture the entire informationrequired to calculate the optimal policy along with thenominal trajectory of a stochastic system. It is therefore nosurprise that these equations provide the evolution of theinformation state (the posterior or conditional distribution ofthe state given the entire history of actions and observations)as the sufficient statistic for decision-making through theBayesian filtering equations.In this paper, through a series of analytic and numericalexamples, we show that the observability Gramian does notgenerally provide an appropriate solution for the problemof planning under uncertain observations. We provide exam-ples for two commonly used nonlinear observation modelsincluding the range and squared-range observation modelsthat provide noisy information regarding the state of thesystem with respect to a set of information sources orlandmarks. The examples show that the OG is insensitive tothe uncertainty parameters of the problem, with none of thethree main covariances, i.e., process, observation or initial,appearing quantitatively. Similarly, we show that the SFIMalso suffers the same problems as the OG.The numerical examples illustrate the performance of sim-ple planning problems when a measure of the OG (or SFIMin special case) is utilized as the optimization objective. Inthese examples, the trace of the error covariance, which rep-resents the sum of mean squared errors along the trajectory,is used as the measure of performance of trajectory. In eachexample, the OG-based trajectory’s performance is evaluatedagainst both an initial trivial path and the optimized path withrespect to the trace of the covariance. The results indicatethat for all three models there are situations where theOG-based trajectory can perform significantly poorly withrespect to these two trajectories, including even the initialtrivial path. In some situations the trajectories produced arequalitatively similar, while their estimation performances arevery different.On the other hand, due to some very special circumstancesOG-based planning may sometimes be close to the optimaloutcome, and we provide such an example too. The above a r X i v : . [ c s . R O ] J a n xamples shows that OG-based planning is not reliable. Oneof the main reasons for usage of the OG-based method hasbeen its relatively simpler computation, in comparison tothe Riccati equation. However, we show that while there isa constant-factor computational difference in terms of thematrix calculations, a careful formulation of the originalproblem can lead to the same “order” of computation as theOG-based problem.We introduce the preliminary notations and definitionsof the Gramian and some OG-based measures in the nextsection. Then, we proceed to the analytic examples inSection III. In Section IV, we provide several formulationsof planning problems and describe the numerical simulationresults. II. P RELIMINARIES
We begin with some preliminary definitions.
Process and observation models:
Let x ∈ X ⊂ R n x , u ∈ U ⊂ R n u and z ∈ Z ⊂ R n z denote the state, control andobservation vectors, respectively. We use boldface variablesto denote the vectors in lower case and matrices in uppercase, respectively. Let f : X × U × R n u → X and h : X → Z denote the general process and observation models: x t +1 = f ( x t , u t , ω t ) , ω t ∼ N ( , Σ ω ) , (1a) z t = h ( x t , ν t ) , ν t ∼ N ( , Σ ν ) , (1b)where { ω t } and { ν t } are zero mean independent, identicallydistributed (i.i.d.) mutually independent random sequences,with N ( m , Σ ) denoting a normal distribution with mean m and covariance Σ . Parameterized Trajectories : Starting with an initial esti-mate, x p := ˆ x , and using a set of unknown control inputs { u pt } K − t =0 , we parametrize the possible feasible nominaltrajectories of the system: x pt +1 := f ( x pt , u pt , ) , ≤ t ≤ K − , z pt := h ( x pt , ) , ≤ t ≤ K. Linearization of the system equations:
We linearize thenonlinear motion and observation models of equation (1)about the parametrized trajectory: ˜ x t +1 = A t ˜ x t + B t ˜ u t + G t ω t , (2a) ˜ z t = H t ˜ x t + M t ν t , (2b)where ˜ x t := x t − x pt , ˜ u t := u t − u pt , and ˜ z t := z t − z pt denotethe state, control and observation errors, respectively, and A t := ∇ x f ( x , u , ω ) | x pt , u pt , , B t := ∇ u f ( x , u , ω ) | x pt , u pt , , G t := ∇ ω f ( x , u , ω ) | x pt , u pt , , H t ( x pt ) := ∇ x h ( x , ν ) | x pt , , M t ( x pt ) := ∇ ν h ( x , ν ) | x pt , . Note that { x pt } Kt =0 , { z pt } Kt =0 , and the Jacobian matriceschange upon change of the underlying control inputs { u pt } K − t =0 . A. Observability GramianObservability Gramian:
Let ˜ A t := Π tτ =0 A τ denote thetransition matrix of the linearized system of (2) starting from time . Then, the ( K +1) -step observability Gramiancorresponding to the nominal trajectory is defined as: Q pK +1 := K (cid:88) t =0 ˜ A Tt H Tt H t ˜ A t . (3)The noise-less system of exactly linear equations is observ-able if and only if rank( Q pn x − ) = n x [1].Note that as the control inputs u pt change, Q pK +1 changes,as well. This has led to a variety of approaches to utilize theOG or some function of the OG as a measure to optimize inthe trajectory optimization problems. One motivating factor,as mentioned above, is the low computational burden ofcomputing the OG. Another motivating factor for using theOG is its proven role in determining the initial state , x p , i.e.,observability property of a deterministic system. However,in the stochastic case, given (partial) information around the initial state, the goal is to find trajectories where the statebecomes more observable along the trajectory (including, inparticular, the final state, which may be important to goal-oriented problems, as opposed to the initial state). Measures of the Gramian:
In several papers, e.g., [13],[19], the following scalar measures of the OG have beenused with various interpretations related to the uncertaintyin the systems: • Determinant of the inverse OG, det(( Q pK +1 ) − ) =det − ( Q pK +1 ) (and sometimes logarithm of it); • Trace of the inverse OG, tr(( Q pK +1 ) − ) ; • Negative trace of the OG, − tr( Q pK +1 ) ; • Inverse of the OG’s minimum eigenvalue, λ − ( Q pK +1 ) ; • Inverse of the OG’s maximum eigenvalue, λ − ( Q pK +1 ) ; • The condition number of the OG, κ ( Q pK +1 ) . B. Standard Fisher Information Matrix
A metric closely related to the Gramian is the SFIM theinverse of which is a lower bound on the minimum attainableestimation covariance for a parameter estimation problem asgiven by the Cram´er-Rao lower bound [25]. The SFIM, F K ,for the system of equations (2) is calculated as [1]: F K = K (cid:88) t =0 ˜ A Tt H Tt Σ − ν H t ˜ A K . (4)Note that in the special case Σ ν = σ I n z with σ > , theSFIM reduces to a weighted OG: F K = 1 σ K (cid:88) t =0 ˜ A Tt H Tt H t ˜ A K = 1 σ Q pK +1 . (5) C. Covariance EvolutionInformation state:
The posterior distribution of x t giventhe history of actions and observations up to time-step t , p X t | Z t ; U t − , X ( x | z t ; u t − , x ) , is referred to as theinformation state. It is a sufficient statistic for the stochasticcontrol problem [6], [7]. In the linear Gaussian case, thecovariance evolution of the information state is specified bythe Kalman filtering equations. The covariance evolution ofthe KF becomes deterministic once the underlying nominalinearization trajectory of the system equations is fixed: P − t = A t − P + t − A Tt − + G t − Σ ω G Tt − , (6a) S t = H t P − t H Tt + M t Σ ν M Tt , (6b) P + t = ( I − P − t H Tt S − t H t ) P − t , P +0 = Σ x . (6c)III. A NALYTIC E VALUATION OF
OG-B
ASED D ESIGNS
In this section, we provide two examples based on com-monly used range and range-squared observation models inorder to compare the amount of information and the differentaspects of the models, such as stochasticity captured by theOG, the SFIM, and the PFIM equations.
System equations:
In the examples of this section, we have x ∈ R , u ∈ R , z ∈ R , and K > . Moreover, the processand observation models are: x t +1 = x t + u t + ω t , ω t ∼ N ( , Σ ω ) , (7a) z t = h ( x t ) + ν t , ν t ∼ N (0 , Σ ν ) , (7b)where { ω t } and { ν t } are zero mean i.i.d. random sequencesthat are mutually independent of each other, x t = [ x t , y t ] T , Σ ω = diag( σ ω x , σ ω y ) , Σ ν = σ ν , and the initial state is dis-tributed as x ∼ N (ˆ x , Σ x ) , where Σ x = diag( σ x , σ y ) .Later in the simulations, we will consider a non-diagonalinitial covariance, as well. Note that except for H t , the otherJacobians of the above system are common to all examples,and are A t = I , B t = I , G t = I , and M t = I . As aresult, ˜ A t = I , t ≥ . A. Range-Only Example
Our first example involves an observation that acquires therange information relative to an information source located atthe origin; i.e., h ( x t ) = r t =: (cid:112) ( x t ) + ( y t ) . The Jacobianof the observation model is H t = ( x t r t , y t r t ) . The OG calculations:
The OG for this system model is Q pK +1 = K (cid:88) t =0 x t r t x t y t r t x t y t r t y t r t . Note that the determinant of the OG is det( Q pK +1 ) = ( K (cid:88) t =0 x t r t )( K (cid:88) t =0 y t r t ) − ( K (cid:88) t =0 x t y t r t ) > , (8)which is positive using the Cauchy-Schwarz inequality, ex-cluding situations where the trajectories of the two coor-dinates are linearly dependent (which includes a situationin which either coordinate’s trajectory is entirely zero, ora situation that the state trajectory is a straight line whoseextension can pass the origin). Therefore, except for thesedegenerate situations this system is observable. The trace ofthe OG is tr( Q pK +1 ) = K + 1 , (9)which is a constant, insensitive to the underlying trajectory. SFIM calculations:
Since the covariance of the obser-vations is a constant and diagonal, the SFIM reducesto the form represented in equation (5), and tr( F K ) = σ − ν tr( Q pK +1 ) = σ − ν ( K + 1) , which is a constant, insen-sitive to the underlying trajectory, just like the trace of the OG. In fact, the SFIM is a constant multiplier of the OG inall subsequent examples, as well. Covariance of the estimation calculations:
The Riccatiequations of (6) for the evolution of the estimation co-variance, in contrast, provide a different perspective thanthe OG and the SFIM. Starting from the initial covariance P +0 = Σ x , the covariance ceases to be a diagonal after justone time step, and its trace t = 1 is: tr( P +1 ) = ( σ x + σ x ω )( σ y + σ y ω ) + ( σ x + σ x ω + σ y + σ y ω ) σ ν ( σ x + σ x ω ) x t r t + ( σ y + σ y ω ) y t r t + σ ν . (10)Unlike in the case of the OG and the SFIM, minimizationbased on the covariance information is indeed sensitive tothe underlying trajectory. In fact, this dependence is revealedafter just one step of the Riccati equation’s update. B. Range-Squared-Only Example
Next, we consider a model that is often used in place ofthe range-only model and show that the behavior of the OGchanges even by a simple squaring of the observation model.We have h ( x t ) = r , with Jacobian given by H t = ( x t , y t ) . The OG calculations:
The OG is Q pK +1 = K (cid:88) t =0 (cid:18) x t x t y t x t y t y t (cid:19) . Its determinant is det( Q pK +1 ) = ( K (cid:88) t =0 x t )( K (cid:88) t =0 y t ) − ( K (cid:88) t =0 x t y t ) > , (11)which is again taken to positive, assuming non-degenerateness. The trace of the OG is tr( Q pK +1 ) = (cid:80) Kt =0 r t , maximizing which suggests trajectories that are farther from the origin. We note that a simple squaring ofthe range produces exactly the opposite result, showing theinappropriateness of an OG-based design and requirementof a careful investigation with the covariance-based design.The SFIM measure also produces similar results. Estimation covariance:
Similarly, given P +0 = Σ x , thetrace of the updated covariance at t = 1 is: tr( P +1 ) =( σ x + σ x ω )( σ y + σ y ω ) r t +( σ x + σ x ω + σ y + σ y ω ) σ ν ( σ x + σ x ω ) x t + ( σ y + σ y ω ) y t + σ ν . (12)This result also shows that, even after just one time step,the filtering equation provides very different and reasonablesolutions than the OG or SFIM measures. Unlike the trace ofthe OG, this result does not suggest a uniform radial move-ment away from the origin; rather, it suggests paths that aredependent and sensitive to the direction of movement takinginto account the uncertainty reductions in those directions. C. Observations
Equations (10) and (12), which represent the trace of thePFIM in each case, provide far more valuable informationthan the any measure of the OG: • The trace of the updated PFIM depends on the underly-ing trajectory. In contrast, the trace of OG can becomea constant regardless of the noise covariances, e.g., (9);
PFIM, takes into account the uncertainties in each direc-tion. In contrast, the OG-based design can be insensitiveto the directions involved; • The trace of the updated covariance is dependent on theprevious covariance of the state estimation; • The trace of covariance depends on both the observationand process noise covariances; and • PFIM’s dependence on the process, observation andprevious (history of uncertainty and prior) covariancesis not uniform in each direction. However, measures ofthe OG are insensitive to such covariances.IV. C
OMPARISON OF T RAJECTORY P LANNING A PPROACHES
In this section, we consider an optimal control problemthat is common in path planning and control problems,particularly in robotic systems. We introduce the generalproblem and describe a commonly used surrogate open-loopoptimal control problem whose cost function is a measure ofthe OG. Finally, we compare the above approaches with atrajectory optimization problem extending our previous workon the Trajectory-optimized Linear Quadratic Gaussian (T-LQG) in [26], [27], which optimizes the underlying trajectoryof an LQG system aiming for the best estimation perfor-mance. This problem utilizes the trace of the covariance asthe optimization objective and is accompanied by a separatefeedback design implemented in the execution of the policy.In a companion paper, we prove the near-optimality of thisframework under a small-noise assumption [27], [28].
Problem 1:
General Stochastic Control Problem
Given x ∼ p ( x ) , solve for the optimal policy: min π E [ K − (cid:88) t =0 c πt ( x t , u t ) + c πK ( x K )] s.t. x t +1 = f ( x t , u t , ω t ) (13a) z t = h ( x t , ν t ) , (13b)where the optimization is over feasible policies, Π , and: • π ∈ Π , π := { π , · · · , π t } , π t : Z t +1 → U ; • u t = π t ( z t ) specifies an action given the entire outputof the system from the beginning up to time-step t , z t ; • c πt ( · , · ) : X × U → R is the one-step cost function; • c πK ( · ) : X → R denotes the terminal cost; and K > . Problem 2:
OG-Based Trajectory Optimization Prob-lem
Solve for the optimal trajectory: min u p K − g ( Q pK +1 ) + K (cid:88) t =1 ( u pt − ) T W ut u pt − s.t. x pt +1 = f ( x pt , u pt , , ≤ t ≤ K − (14a) x p = E x [ p ( x )] (14b) || x pK − x g || < r g (14c) || u pt || ≤ r u , ≤ t ≤ K, (14d)where the optimization is over feasible controls, g : R n x × n x → R represents a specific operation on the OG,such as trace, determinant, etc., W ut (cid:23) , r u > , and r g > and x g ∈ X specify the goal region. Problem 3:
T-LQG Planning Problem [27]
Solve for theoptimal linearization trajectory of the LQG policy: min u p K − K (cid:88) t =1 [tr( P + b pt ) + ( u pt − ) T W ut u pt − ] s.t. P − t = A t − P + t − A Tt − + G t − Σ ω t − G Tt − (15a) S t = H t P − t H Tt + M t Σ ν t M Tt (15b) P + t = ( I − P − t H Tt S − t H t ) P − t , P +0 = Σ x (15c) x p = E x [ p ( x )] (15d) x pt +1 = f ( x pt , u pt , , ≤ t ≤ K − (15e) || x pK − x g || < r g (15f) || u pt || ≤ r u , ≤ t ≤ K, (15g)where the optimization is over feasible controls, and equa-tions (15a)-(15c) represent one iteration of the Riccati equa-tion to calculate the first term of the objective.We now describe the performance of the above ap-proaches. We perform several numerical simulations forvarious initial, process and observation uncertainties for bothof the problems 2 and 3 and all three observation models.First, we provide an example for the range-squared obser-vation model, where we show that the trajectory providedby the OG-based problem of 2 can significantly under-perform in terms of reducing the estimation uncertainty.We show that planning based on the OG can result inundesirable trajectories for these partially observed problems,which stems from the fact that the OG is insensitive to theuncertainty parameters of the problem and provides the sameresult regardless of the changes in the three covariances.Next, we provide an example for the other model wherequalitatively the output trajectories of the two problemsresemble each other, but the covariance evolution results inthe slight differences in the state trajectory contributing to asignificant difference in the qualities of the trajectories interms of the filters’ performances. Lastly, we provide anexample showing that when the intensity of noises tendsto zero (particularly, if the sensor noise is very low), theperformances of the OG-based and covariance-based trajec-tories tend to be close to each other. All our simulations areperformed in MATLAB 2016b using the fmincon solver.For all the figures that depict the state trajectories: • x ∈ R , u ∈ R , z ∈ R , and K = 7 ; • W ut = 0 I , r u = 0 . , r g = 0 . and x g = ( − , . T ,which is indicated by a purple circle in the figures; • The units of the axes are in meters; • The initial estimate is ˆ x = ( − . , − . T , which isindicated by a green diamond in the figures; • The information sources are located at the centers ofthe light areas in the figures; • The initial trajectory for the solver, indicated witha dashed orange line, consists of three straight seg-ments passing through ( − . , − . T , ( − . , . T , ( − . , . T , and ( − , . T . Hence, the determin-istic system is observable for all three models; and • The optimized trajectory is shown by a solid cyan line. a) Range-squared, OG-Based (b) Range-squared, Cov-Based(c) Range, OG-Based (d) Range, Cov-BasedFig. 1. Simulation results for the planning problem 2 based on the conditionnumber of the OG for range-squared and range observation models in (a)and (c), and the planning problem 3 using the trace of the covariance forrange-squared and range observation models in (b) and (d), respectively.The information sources are located in the centers of the light areas. Thedashed orange line represents the initial trajectory, while the solid cyan lineshows the optimized trajectory.(a) Range-squared (b) RangeFig. 2. Evolution of the trace of the covariance along the trajectory for theinitial trajectory, optimization based on the OG measure, and optimizationbased on the covariance measure of the trajectories in Fig. 1.
A. Range-Squared-Only Observations
Figures 1a and 1b show the results of the simulations forthe range-squared-only observation model using the condi-tion number of the OG and the trace of the covariance alongthe trajectory as the cost function, respectively. Informationsources are at (0 . , T , (0 . , . T , and (2 , T , and Σ x = (cid:18) .
025 0 . .
002 0 . (cid:19) , Σ ω = (cid:18) . . . . (cid:19) , Σ ν = 0 . . Figure 2a shows the evolution of the trace of covariancealong the trajectories. While it is expected that the trajectorydeigned based on the covariance evolution performs betterthan the other ones, it is surprising to observe that the OG-based trajectory actually under-performs the initial trajectoryas well. Even though we have only shown the results of thesimulation for the condition number of OG, the interested (a) OG-Based Trajectory (b) Cov-Based TrajectoryFig. 3. Range-only observation model: a) The optimized state trajectory ofthe planning problem 2 using the condition number of the OG as the costfunction, b) The optimized state trajectory of the planning problem 3 usingthe trace of the covariance as the cost function. The information sources arelocated in the centers of the light areas. The dashed orange line representsthe initial trajectory, while the solid cyan line shows the optimized trajectory.Fig. 4. Range observation model. Evolution of the trace of the covariancealong the trajectory for the initial trajectory, optimization based on theOG measure, and optimization based on the covariance measure of thetrajectories in Fig. 3. reader can find a more detailed set of experiments withother measures of the Gramian in a companion technicalreport [29], which parallel the results provided here. Thequantitative result of Fig. 2a, along with the qualitativedifference in the trajectories as indicated in Fig. 1, indicatethat a measure of the OG is not a reliable measure tooptimize in a problem with initial, process and observationuncertainties.
B. Range-Only Observations
Figures 1c and 1d show the results of the similar simula-tions for the range-only observation model with the conditionnumber of the OG and the trace of the covariance as the costfunction, respectively. Information sources are at (0 . , T ,and (0 . , . T , and Σ x = (cid:18) .
25 00 0 . (cid:19) , Σ ω = (cid:18) . (cid:19) , Σ ν = 0 . . Figure 2b shows the covariance evolution for the trajectoriesof this simulation, which resembles the results of Fig. 2a.
C. Another Range-Only Scenario
Last, Figs. 3a and 3b show the results of another setof simulations for the range-only observation model usingcondition number of the OG and the trace of the covari-ance, respectively. Information sources are located at (0 , T , . , . T , and (0 . , . T , and Σ x = (cid:18) .
02 00 0 . (cid:19) , Σ ω = (cid:18) . . (cid:19) , Σ ν = 0 . . In this experiment, the reduced noise covariances, partic-ularly the observation covariance, lead to the high quality ofmeasurements from a broad class of trajectories. As a result,the trace of covariance evolution of Fig. 3 indicates only aslight difference between the three trajectories.
Remark:
It should be noted that in all the figures, sincethe state trajectories are softly constrained to reach to thesame goal region at the end of the navigation, the covarianceevolutions converge to each other towards the end of thetrajectories. This is due to the fact that in the Bayesianfiltering, the latest observations (which arise from the sameregion in the state space) carry a higher weight than the priorhistory. As a result, in comparing the covariance evolutions,the variations in the behavior along the entire trajectoryis of concern since a highly certain trajectory can lead tosafer navigation, particularly, in a complex environment withobstacles, banned areas or multiple agents.
Remark:
Finally, note that the simulation times to solvethe optimization problem for all cases are of the same order,which stems from the fact that the computation complexityof both the problems 2 and 3 is O ( Kn x ) [27].V. C ONCLUSION
In this paper, we have investigated a well-known heuristicemploying the observability Gramian in planning underobservation uncertainty. We have utilized two common ob-servation models and shown that, in general, the observabilityGramian (and the closely-related standard Fisher informationmatrix) fail to capture many aspects of the models includ-ing the initial, process, and observation uncertainties. As aresult, based on changes in those models, we showed usinganalytic and numerical examples that planning based on theobservability Gramian can provide trajectories that are verydifferent in terms of the estimation performance from theoptimal plans based on the estimation covariance of theproblem. R
EFERENCES[1] P. S. Maybeck,
Stochastic models, estimation, and control . Academicpress, 1982, vol. 3, pp. 45–48, 238–241.[2] K. Yasuda and R. E. Skelton, “Assigning controllability and observ-ability gramians in feedback control,”
Journal of Guidance, Control,and Dynamics , vol. 14, no. 5, pp. 878–885, 1991.[3] U. Vaidya, “Observability gramian for nonlinear systems,” in
Decisionand Control, 2007 46th IEEE Conference on . IEEE, 2007, pp. 3357–3362.[4] B. Southall, B. F. Buxton, and J. A. Marchant, “Controllability andobservability: Tools for kalman filter design.” in
BMVC , 1998, pp.1–10.[5] A. J. Krener and K. Ide, “Measures of unobservability,” in
Decisionand Control, 2009 held jointly with the 2009 28th Chinese ControlConference. CDC/CCC 2009. Proceedings of the 48th IEEE Confer-ence on . IEEE, 2009, pp. 6401–6406.[6] P. R. Kumar and P. P. Varaiya,
Stochastic Systems: Estimation,Identification, and Adaptive Control . Englewood Cliffs, NJ: Prentice-Hall, 1986.[7] D. Bertsekas,
Dynamic Programming and Optimal Control: 3rd Ed.
Athena Scientific, 2007. [8] M. Rafieisakhaei, A. Tamjidi, S. Chakravorty, and P. Kumar, “Feed-back Motion Planning Under Non-Gaussian Uncertainty and Non-Convex State Constraints,” in . IEEE, 2016, pp. 4238–4244.[9] R. Platt, “Convex receding horizon control in non-gaussian beliefspace,” in
Algorithmic Foundations of Robotics X . Springer, 2013,pp. 443–458.[10] R. Platt, R. Tedrake, L. Kaelbling, and T. Lozano-Perez, “Belief spaceplanning assuming maximum likelihood observations,” in
Robotics:Science and Systems (RSS) , 2010.[11] J. Van Den Berg, P. Abbeel, and K. Goldberg, “Lqg-mp: Optimizedpath planning for robots with motion uncertainty and imperfect stateinformation,”
The International Journal of Robotics Research , vol. 30,no. 7, pp. 895–913, 2011.[12] D. Georges, “Energy minimization and observability maximizationin multi-hop wireless sensor networks,”
IFAC Proceedings Volumes ,vol. 44, no. 1, pp. 13 918–13 923, 2011.[13] B. T. Hinson, “Observability-based guidance and sensor placement,”Ph.D. dissertation, University of Washington, 2014.[14] B. T. Hinson, M. K. Binder, and K. A. Morgansen, “Path planningto optimize observability in a planar uniform flow field,” in
AmericanControl Conference (ACC), 2013 . IEEE, 2013, pp. 1392–1399.[15] J. D. Quenzer and K. A. Morgansen, “Observability based controlin range-only underwater vehicle localization,” in
American ControlConference (ACC), 2014 . IEEE, 2014, pp. 4702–4707.[16] M. Travers and H. Choset, “Use of the nonlinear observability rankcondition for improved parametric estimation,” in
Robotics and Au-tomation (ICRA), 2015 IEEE International Conference on . IEEE,2015, pp. 1029–1035.[17] L. DeVries and D. A. Paley, “Wake sensing and estimation for controlof autonomous aircraft in formation flight,”
Journal of Guidance,Control, and Dynamics , vol. 39, no. 1, pp. 32–41, 2015.[18] L. DeVries, S. J. Majumdar, and D. A. Paley, “Observability-basedoptimization of coordinated sampling trajectories for recursive esti-mation of a strong, spatially varying flowfield,”
Journal of Intelligent& Robotic Systems , vol. 70, no. 1-4, pp. 527–544, 2013.[19] A. K. Singh and J. Hahn, “Determining optimal sensor locations forstate and parameter estimation for stable nonlinear systems,”
Industrial& engineering chemistry research , vol. 44, no. 15, pp. 5645–5659,2005.[20] R. Platt, L. Kaelbling, T. Lozano-Perez, and R. Tedrake, “Non-gaussian belief space planning: Correctness and complexity,” in
Robotics and Automation (ICRA), 2012 IEEE International Conferenceon . IEEE, 2012, pp. 4711–4717.[21] P. Tichavsky, C. H. Muravchik, and A. Nehorai, “Posterior cram´er-raobounds for discrete-time nonlinear filtering,”
IEEE Transactions onsignal processing , vol. 46, no. 5, pp. 1386–1396, 1998.[22] N. Thacker and A. Lacey, “Tutorial: The likelihood interpretation ofthe kalman filter,”
TINA Memos: Advanced Applied Statistics , vol. 2,no. 1, pp. 1–11, 1996.[23] K. Bastani, B. Barazandeh, and Z. J. Kong, “Fault diagnosis in multi-station assembly systems using spatially correlated bayesian learningalgorithm,”
Journal of Manufacturing Science and Engineering , vol.140, no. 3, p. 031003, 2018.[24] M. Lei, C. Baehr, and P. Del Moral, “Fisher information matrix-based nonlinear system conversion for state estimation,” in
Controland Automation (ICCA), 2010 8th IEEE International Conference on .IEEE, 2010, pp. 837–841.[25] G. Casella and R. L. Berger,
Statistical inference . Duxbury PacificGrove, CA, 2002, vol. 2.[26] M. Rafieisakhaei, S. Chakravorty, and P. R. Kumar, “T-LQG: Closed-Loop Belief Space Planning via Trajectory-Optimized LQG,” in .IEEE, 2017, pp. 649–656.[27] M. Rafieisakhaei, S. Chakravorty, and P. Kumar, “Belief Space Plan-ning Simplified: Trajectory-optimized LQG (T-LQG),” arXiv preprintarXiv:1608.03013 , 2016.[28] M. Rafieisakhaei, S. Chakravorty, and P. R. Kumar, “A Near-OptimalDecoupling Principle for Nonlinear Stochastic Systems Arising inRobotic Path Planning and Control,” in . IEEE, 2017.[29] M. Rafieisakhaei, S. Chakravorty, and P. R. Kumar, “On the Use of theObservability Gramian for Robotic Path Planning Under ObservationUncertainty,” arXiv preprint arXiv:arXiv preprint arXiv: