Use of the Nonlinear Observability Rank Condition for ImprovedParametric Estimation

Matthew Travers and Howie Choset

Abstract— The correct way to design controllers for dynamicrobots is still very much an open question. This is in alarge part due to the complexity and uncertainty in modelingtheir nonlinear dynamics. In this work, we focus on derivingconcise dynamic expressions for a particular class of robotsthat can be used to better reduce uncertainty with respectto unknown parameters in realtime. We accomplish this byusing an extended Kalman filtering framework in conjunctionwith an online controller that continuously maximizes a localmeasure of nonlinear observability. The main novel contributionof this work is that we directly use the nonlinear observabilityrank condition to derive the measure of observability at eachtime step. We are able to make this extension in part byfocusing on serial-chain systems and exploiting the geometricstructure in their dynamic models. In particular, we deriveconcise, closed-form and exact analytical representations for theforward dynamics, linearization, and nonlinear observabilityrank condition of a fixed-base serial manipulator with activelycontrolled elastic joints. An example is presented in whichthe spring constants and damping coefficients for a series-elastic actuated manipulator are estimated using the onlineobservability maximizing techniques we derive.

I. INTRODUCTION

The inability to precisely model and measure the proper-ties of a robot makes designing controllers for them almost asmuch of an art as it is a science. Advances in dynamic mod-eling, sensor development, and nonlinear estimation havehelped, but measurement and modeling uncertainties con-tinue to be barriers to the development of effective nonlinearcontrollers for robots. This work focuses on how advances inmodeling and estimation can be combined in a novel way tohelp address modeling uncertainty; specifically, parametricuncertainty in dynamic systems. We present a method whichdirectly uses properties of the nonlinear observability rankcondition to modify an online controller, ultimately enablinga set of unknown system parameters to converge to theirtrue values faster than a standard filtering-based parameterestimation approach.

Observability in both linear and nonlinear systems istraditionally treated as a discrete property; the system eitheris or is not observable. Recently, several researchers [8],[5] have developed the notion that there is a degree towhich a system is observable and that this property is ameasurable quantity. Hinson et. al. [5] have moved one stepfurther, asking the question whether, for controlled systems,controllers can be designed to maximize observability andsubsequently improve online estimation.

In this paper we present an approach which is relatedto that in [5]. Our main novel extension is that we usethe nonlinear observability rank condition in place of the

Fig. 1: Quasi-fixed base series-elastic actuated serial-chainmanipulator.

observability gramian as our basis for computing the degreeto which a system is observable, or rather its observabilitymeasure. The advantage of this approach is at this pointprimarily computational. As we show in the context of serial-chain manipulators with series-elastic joints, it is possibleto obtain the nonlinear observability rank condition forrelatively complicated systems with an arbitrary number ofjoints in exact closed form. For moderately sized systemsthis allows the observability measure to be computed andpartiality optimized using an online controller; a major ad-vantage in uncertain environments where desired objectivesmay be subject to sudden changes. The authors in [8] and[9] do present a method for approximating the observabilitygramian that grows linearly with the number of states inthe system, but requires two numerical integrations perevaluation of the gramian in addition to the calculation of thedynamics required for the method presented in this paper.

Deriving a concise representation for a serial-chain manip-ulator’s dynamics is not novel, but is critical to perform thecalculation of the observability rank condition in realtime. Aportion of this paper thus reviews results from the literatureon computationally efficient forward dynamic calculations.We use a pre-existing batch Newton-Euler formulation toderive analytic forms for both the generalized inertia as wellas Coriolis and centripetal terms which appear in the second-order dynamics of an elastic-joint serial manipulator. We usethese expressions to analytically linearize the system. Wethen show that the nonlinear observability rank condition forthese systems can be written entirely in terms of the forward

2015 IEEE International Conference on Robotics and Automation (ICRA)Washington State Convention CenterSeattle, Washington, May 26-30, 2015

978-1-4799-6922-7/15/$31.00 ©2015 IEEE 1029

dynamics and linearization.To demonstrate the benefit of our approach, we sim-

ulate a six-link series-elastic actuated (SEA) serial-chainmanipulator with rotational joints (originally inspired by thequasi-fixed base modular snake robot configuration shownin Figure 1 [15]). We assume that we are able to measureonly the motor- and load-side angular positions, but wishto estimate the positions, velocities, as well as the springconstants and damping coefficients for each of the actuators.We use an extended Kalman filter (EKF) framework toestimate the full state of the system; including the positions,velocities, and spring parameters [10]. We present resultswhich show that the filter converges to the set of originallyunknown parameter values faster when the observabilitymeasure optimization is incorporated into the controller asopposed to when it is not. A comparison is provided.

II. PROBLEM DEFINITION

Consider the system

˙x = f(x,S, u(t))

y = h(x,S, u(t)),(1)

where x ∈ Rn, S ∈ Rp is a vector of constant but unknownsystem parameters, u(t) ∈ Rnu is a vector of control inputs,and y ∈ Rm is a vector of the measured system outputs.The problem we wish to solve is to estimate the set ofparameters S as fast as possible in the presence of processand measurement uncertainty.

For notational convenience, throughout the remainder ofthe paper we will rewrite (1) in terms of the extended statex = (x,S) ∈ Rn where n = n + p. The system dynamicsthus become

x = f(x, u)

y = h(x, u),(2)

where f(x, u) = (f(x,S, u), 0p).

III. BACKGROUND

A. Nonlinear Observability

We provide a general description of nonlinear observ-ability. A thorough derivation and more in depth explana-tion is left to [4], [6], [17]. We start by making severalassumptions on the inputs and outputs of the systems weare interested in. In particular, the output function h(·) isassumed to be an explicit function of the input, i.e., h(x, u) =(h1(x, u), h2(x, u), . . . , hm(x, u)). We also require that theinput functions be at least jth-order differentiable. In generalj will be defined by the particular system; we assume onlythat j ≥ 1. We define the following expressions in terms ofthe Lie derivative, L·(·),

φ0i = hi

φji = Lfφj−1i =

∂φj−1i

∂xf +

j−1∑k=0

∂φj−1i

∂u(k)u(k+1) = y

(j)i .

We also define the map Φ(x,u) : Rn × Rnu → Rn

Φ(x, u) = (h1, φ11, . . . , φ

k1−1i , . . . , hm, φ

1m, . . . , φ

km−1m )T ,

(3)where,

∑mi=1 ki = n [3].

Definition 1 The system (2) is locally observable if themapping Φ(x,u) is invertible with respect to the state xand its inverse is smooth for all (x,u) in a local subset ofRn × Rnu , or rather, if

rank(∂Φ(x,u)

∂x

)= n. (4)

Equation (4) is the nonlinear observability rank condition.

There are several important things to note about Φ. First,as Hinson et. al. point out [5], the derivatives of the com-ponents φj−1

i with respect to the controls u and derivativesthereof appear explicitly in Φ and subsequently in the (4).Fundamentally, this reflects the fact that for a nonlinearsystem the control is implicitly coupled to the estimation.This fact fundamentally explains why the results presentedin [5] and in this paper are possible. Secondly, the onlyrequirement on the individual ki in (3) is that they sum ton. This implies that Φ is in general not unique. We will usethis fact in Section IV to facilitate the calculation of (4).

B. Dynamics of Chained Systems

This work was originally inspired by the desire to control afixed-base series-elastic actuated serial-topology system withrotational joints. Based on this desire we restrict further in-vestigation to this particular class of serial-chain systems. Wefirst combine several previous results which make it possibleto concisely express the dynamics of these mechanisms. Wethen show how these expressions can be used to analyticallylinearize the associated second-order dynamics. In SectionVI we use these results to express both Φ as well as thenonlinear observability rank condition (4) in explicit closed-form for up the N joints.

1) Geometric Preliminaries: The material in this sectionis drawn from several different sources. Details are providedwhere necessary to present a clear description, but furtherexplanations are left to [11], [13], [1], [2]. We start bydefining a rigid body transformation g ∈ SE(3) betweenframes i and k in terms of the matrix exponential operator

gi,k(θ) = exp(ξiθi) exp(ξi+1θi+1) . . . exp(ξkθk)gi,k(0)

=

[R q0 1

], (5)

where R ∈ SO(3) is a three-dimensional rotation matrix,q ∈ R3, ξj is a twist [11], and the operator · takes the vectorξj = (vj , ωj) ∈ R6 into the R4×4 matrix representation

ξj =

[ωj vj0 0

]. (6)

1030

There is a second · operator in (6) which maps ω ∈ R3 intothe set of 3× 3 skew symmetric matrices, i.e.,

ω =

0 −ω3 ω2

ω3 0 −ω1

−ω2 ω1 0

. (7)

We will not explicitly differentiate between these operatorsin the rest of this paper, but it will be clear from contextwhich instance is being used.

Several of the calculations in the forward dynamics andlinearization of a serial-chain mechanism depend on the di-rectional derivatives of the individual link’s forward kinemat-ics. The chained matrix exponentials in (5) make calculatingthese derivatives relatively simple, i.e., for j ≤ k

∂

∂θjg1,k(θ) = exp(ξ1θ1) exp(ξ2θ2) . . .

ξj exp(ξjθj) . . . exp(ξkθk)g1,k(0), (8)

and i ≤ j ≤ k

∂

∂θi

∂

∂θjg1,k(θ) = exp(ξ1θ1) exp(ξ2θ2) . . . ξi exp(ξiθi) . . .

ξj exp(ξjθj) . . . exp(ξkθk)g1,k(0). (9)

The adjoint transformation transforms elements ξ of theLie algebra se(3) (which is associated with SE(3)) from onecoordinate frame to another. Given some g ∈ SE(3), theadjoint transformation Adg : R6 → R6 has the matrix form

Adg =

[R qR0 R

]There is a second adjoint transformation which maps el-ements of the dual Lie algebra se(3)∗ between differentcoordinate frames as well

Ad∗g =

[RT −RT q0 RT

]In addition to adjoint transformations there are adjoint

representations, which map the action of the Lie algebraonto itself, ξ ∈ se(3), adξ : se(3) → se(3), which has thematrix representation

adξ(·) =

[ω v0 ω

]. (10)

The adjoint representation can also be defined in terms of theLie bracket, adξη = [ξ, η] for ξ = (v1, ω1) and η = (v2, ω2)both in se(3). The adjoint representation also has a dual,which maps the action of the Lie algebra onto the dual Liealgebra; the matrix representation for this action is

ad∗ξ(·) =

[−ω 0−v −ω

].

2) Newton-Euler Dynamics: There are a number of dif-ferent computational methods which use a Newton-Eulerframework to efficiently calculate both numerical as wellas analytic forward dynamic expressions for serial-chainmechanisms. We use the version found in [13]. Newton-Euler formulations consist of a two-step iterative process.In the first iteration the velocities and accelerations of eachlink are recursively propagated from the mechanism’s baseto its end effector. In the second iteration, the forces andjoint torques are recursively propagated back from the endeffector to the base. More specifically, for an N -link serialchain, we initialize the iterative process with

ξ01 = ξ0

1 = FN+1 = 0, (11)

where, ξ0j is a body velocity and F is a wrench [11]. The

forward recursion proceeds from i = 1 to N such that

gi−1,i = exp(ξiθi)gi−1,i(0)

ξ0i = Adg−1

i−1,iξ0i−1 + ξiθi

ξ0i = ξiθ + Adg−1

i−1,iξ0i−1 + adAd−1

gi−1,iξ0i−1

ξiθi.

(12)

Following the forward recursion, the backward recursionproceeds from i = N to 1,

Fi = Ad∗g−1i,i+1

Fi+1 + Jiξ0i − ad∗ξ0i Jiξ

0i

τi = ξTi Fi.(13)

Following the results in [12], and the references therein,the expressions in (11), (12), and (13) can be written asa large algebraic system and rearranged using block-rowelimination as M A−T 0

A−1 0 ξ0 0 M

−ξ0

F

θ

=

b−aτ − C

, (14)

where, ξ0 = [ξ01 ξ

02 . . . ξ

0N ]T , ξ = [ξ1 ξ2 . . . ξN ]T , and due

to our initialization in (11), M and C in the last block rowexactly correspond to the generalized inertia and Coriolis andcentripetal matrices respectively. Both a and b in (14) areblock expressions which can be derived from (12) and (13)

a =

ad∗ξ01J1ξ

01

ad∗ξ02J2ξ02

...ad∗ξ0nJnξ

0n

b =

0

adAd−1g2ξ01ξ2θ2

...adAd−1

gn−1,nξ0nξnθn

(15)

The expression in (14) leads to the following matrixdecompositions for M and C [12]

M = di6(ξ)TATMA di6(ξ)

C = di6(ξ)TAT (MAa+ b),(16)

where, di6 : R6N×1 → R6N×N is an operator whichtakes individual ξi vectors in and places them on the blockdiagonal of a 6N ×N matrix. Note that much of therelated dynamics literature is concerned with manipulatingthe expressions in (14) to derive numerically efficient algo-rithms. We are not necessarily interested in directly pursuing

1031

computational efficiency, in this sense, in this work. We arehowever interested in using the the matrix decompositionsin (16) to facilitate the computations of the linearization ofthe dynamics in (2).

C. Linearization

1) Derivatives of M and C : The linearization of (2) isfound by taking the derivatives of f(·) and h(·) with respectto the extended state vector x = (θ, θ,S). Using the matrixexpressions for M and C in (16) it is possible to expressthese derivative concisely and in closed form. We note thatthe authors in [7] provide results which linearize the second-order dynamics of serial-chain mechanisms. Their results areultimately equivalent to those included in this section, exceptwe derive simple closed-form solutions which are intuitivelyexpressed using the geometric notion found in [13] and [11].Our results make it much easier to, for example, compute thehigher-order derivatives of the dynamics.

It is helpful to first define several matrix relationships andoperators which appear throughout the linearization calcu-lations. We start by defining the operator di6ad : R6N×1 →R6N×6N s.t.,

di6ad

ξ1ξ2...ξn

=

adξ1(·) 0 . . . 0

0 adξ2(·) . . . 0...

.... . .

...0 0 . . . adξn(·)

.(17)

The dual to (17), di6ad∗ , has the same diagonal form butreplaces adξ with ad∗ξ . The body velocities ξ0

i can be writtenin a single stacked vector using the matrix relationship

ξ01

ξ02......ξ0n

=

I 0 . . . 0Adg−1

2I . . . 0

Adg−12,3

Adg−13

. . . 0

......

. . ....

Adg−12,n

Adg−13,n

. . . I

ξ1θ1

ξ2θ2

...

...ξnθn

.(18)

Note that (18) can concisely be written ξ0 = Aψ, where ψ =[ξ1θ1 ξ2θ2 . . . ξnθN ]T . We also define the block diagonalmatrix Ij whose entries are all zero except for a 6×6 identitymatrix in the j-th block of the main diagonal; Ij serves asa block selection matrix.

The expressions in (17) and (18) can be used to rewrite aand b in (15) as

a = di6ad∗(Aψ)MAψ

b = di6ad((A− I)Aψ)ψ.(19)

Equation 19 allows M and C in (16) to be written in termsof ψ, A, and M only. Further, A is a function of θ only, ψis a function of θ only, and M is a constant matrix. Thus,once the derivatives ∂A/∂θ and ∂ψ/∂θ are determined,the derivatives ∂M/∂θ, ∂M/∂θ, ∂C/∂θ, and ∂C/∂θ aretrivially calculated using the chain rule.

The derivatives ∂A/∂θ and ∂ψ/∂θ can be expressed astensors of dimension R6n×6n×n and R6n×1×n respectively.

Calculating ∂ψ/∂θ is trivial, as each of the n different 6n×1components has the form

∂ψ

∂θi= [0 . . . ξi . . . 0]

T (20)

Calculating ∂A/∂θ is facilitated by using the followingexpression [13]

∂

∂θkAdji (ξi−1) =

{Adjk([Adk−1

i ξi−1, ξk]) i ≤ k ≤ j0 otherwise.

(21)Recalling the relationship between the adjoint representationand Lie bracket (and properties of the Lie bracket), the topterm on the right hand side of (21) can be rewritten

Adjk([Adk−1i ξi−1, ξk]) = −Adjk

(adξk(Adk−1

i ξi−1)). (22)

From the expression in (22), the derivative of the adjointrepresentation can be defined in terms of the operator∂Adji (·)/∂θk = −Adjk

(adξk(Adk−1

i ))

. This expression ap-pears to be relatively complicated, but it allows each of the6n× 6n terms ∂A/∂θi to be written concisely as

∂A/∂θi = AIidi6ad(ξ)(I −A). (23)

As stated above, the expressions in (20) and (23) can beused along with the chain rule to derive explicit closed-formexpressions for the directional derivatives of M and C.

2) Linearization of elastic joint mechanisms : We assumethat the second-order dynamics of a serial chain manipulatorwith N elastic joints has the form [18]

θλ =M−1(τS − C + G) (24)

θµ = 1/Jµ(u− τS), (25)

h(θ, θ) = θ, (26)

where θ = (θλ, θµ) and θ = (θλ, θµ) contain both the load,θλ, and motor, θµ, angles and velocities respectively. Theterms G, τS , and u correspond to the dynamic contributionsdue to gravity, the elastic elements located between joints,and the motor control torques. A linear model with dampingis used to model the spring torque, i.e., for each of the Njoints τ iS = ki(θ

iµ − θiλ) + bi(θ

iµ − θiλ). The 2N second-

order terms in (24) and (25) can be written along with the2N first-order link velocities and 2N parameter velocitiesas a 6N -dimensional first-order system in the form (2). Forthe purpose of computing the linearization we will treat (24)and (25) independently, then merge these results back intothe full state linearization. The linearization of the 4N first-order dynamic terms is trivial to compute.

The derivatives of θλ and θµ are defined in terms of thederivatives ofM and C defined in Section III-C.1 well as thederivatives of τS and G with respect to the full state vectorx. The linear spring model makes computing the derivativesof τS and subsequently θµ trivial. These derivatives are notincluded here due to space considerations. To compute thederivatives of G, we first leverage the structure in (8) and (9),

Gj =

N∑k=j

mjg∂

∂θjgh1,k(θ), (27)

1032

where mi is the mass of link i, g is the magnitude of gravity,and the superscript h denotes the position element of g1,j(θ)corresponding to the height above an inertial frame with zeropotential due to gravity. The derivatives of (27) with respectto θi, for j ≤ i, are then defined to be

∂

∂θiGj =

N∑k=i

mjg∂

∂θi

∂

∂θjgh1,k(θ). (28)

Note that (28) is only valid for j ≤ i, but that the derivativesare symmetric, i.e., ∂

∂θiGj = ∂

∂θjGi.

One final point is that the inverse of M appears in(24), where Section III-C.1 only discuss computing thelinearization of M. This can be resolved using the simplematrix relationship ∂

∂xM−1 = −M−1 ∂

∂x (M)M−1. It iswell known that computing the inverse of M is an O(N3)operation, and thus special consideration needs to be appliedas the number of joints N gets large. Several methods whichmake it possible to efficiently invert M do exist, but for thepurpose of this paper are left to the references [14], [16].

IV. CALCULATION OF NONLINEAR OBSERVABILITYRANK CONDITION

Calculating the mapping Φ in (3) can potentially be verydifficult to obtain in closed-form. For the serial elastic-joint system described in (24)-(26), the linear expressionfor the motor-side dynamics in (25) along with the explicitlinearizations of θµ and θλ can be exploited to address thisdifficulty. We begin our derivation of Φ by defining

Φ =[λφ

01 λφ

11 . . . λφ

0N λφ

1N

µφ01 µφ

11 µφ

21 µφ

31 . . . µφ

0N µφ

1N µφ

2N µφ

3N

]T, (29)

where λφ0j = θjλ and µφ

0j = θjµ. Notice that (29) contains

only zeroth and first-order Lie derivatives with respect to theload-side position measurements but has up to third-orderderivatives with respect to the motor position measurements.As mentioned in Section III-A this choice of Φ is notunique. This specific form was chosen to take advantageof the relatively simple expressions for θµ. Notice also thatmost of the components in (29) are trivial to compute, i.e.,{µ,λ}φ

0i = θi{µ,λ}, {µ,λ}φ

1i = θi{µ,λ}, and µφ

2i = θiµ. Each of

these terms are directly measured or are able to be estimatedusing the framework described in Section V.

The only expressions in (29) which are not measured orable to be estimated directly are the µφ3

i ’s. To calculate theseexpressions, consider first a single joint system where x =(θλ, θµ, θλ, θµ, kS , bS) and the second-order load and motordynamics have the forms (24) and (25) respectively. We makethe assumption that the motor controller has the form u =kP (θdµ − θµ), where kP ∈ R+. For this system,

µφ3 = Dxθµ ◦ x+ u, (30)

where Dxθµ ◦ x = bS(θλ − θµ) + kS(θλ − θµ) and u =kP (θdµ− θµ). The expression in (30) is a sum of componentsthat appear in the extended state dynamics x = f(x, u), i.e.,(30) contains only terms which can be calculated in closedform using the results of Sections III-C.1 and III-C.2.

The results for this one-dimensional example are directlyapplicable to systems with more degrees of freedom as longas the load and motor side dynamics have the forms (24) and(25) respectively. This is due to the fact that each link’s motordynamics are decoupled from the rest of the mechanism upto the third-order Lie derivative; µφ3

i for each link will thushave the exact form as (30).

Using (30), the nonlinear observability rank condition (4)can be computed in closed form using only the results inSections III-C.1 and III-C.2. In particular, the highest-orderterms that appear in Φ are the θλ’s and θµ’s. In computing(4), we need only the closed-form linearizations of theseterms to explicitly calculate ∂Φ/∂x. The calculations neces-sary to obtain these linearizations are presented in SectionIII-C.2. Each of the lower-order terms that appear in (4)can directly be measured, estimated, or calculated from thesystem’s forward dynamics (2).

V. PARAMETER ESTIMATION AND ONLINE CONTROL

The main novel contribution of this work is that we use theresults from the previous sections of this paper to obtain aclosed-form expression for the nonlinear observability rankcondition which is then uses to derive a local measure ofobservability. We subsequently optimize this observabilitymeasure at each time step in an online controller to enhancethe performance of a recursive filter. The objective of thisapproach is to make the estimation of a set of unknownspring parameters in a serial-chain elastic-joint mechanismconverge as fast as possible to their true values. We ac-complish by simultaneously running an EKF to estimatethe extended state vector x, which includes the unknownspring parameters k and b, while locally performing gradientdescent on a measure of the local observability. There are atleast two different measures of observability found in theliterature [8]. Following [5], we use the ratio of the largestto the least singular value of an observability rank conditionmatrix; this has previously been referred to as the localestimation condition number. This choice was motivated byour goal to estimate all of the the unknown spring parameterssimultaneously.

To maximize the observability of the spring parameters,we minimize the nonlinear estimation condition numberof the matrix block in (4) that corresponds to the springparameters; for our example system this is the bottom2N × 2N block on the main diagonal of ∂Φ/∂x. Thislocal optimization has the effect of forcing the minimumand maximum singular values closer together, i.e., it forcesthe observability to be more uniform over all the parameters.This objective also forces the minimum singular value awayfrom zero, and thus does implicitly maximize observability(using the proximity of the minimum singular value to zeroas an objective).

For our simulated results, we make the same assumptionon u as we did in (30), i.e., u = kP (θdµ−θµ), except that herewe assume the reference trajectory θdµ is not fixed. We locallyminimize the estimation condition number at each time stepby first computing its gradient with respect to the motor-side

1033

0 20 40 60 800

5

10

15b1 vs. t

t

b1

0 20 40 60 800

5

10

15b2 vs. t

t

b2

0 20 40 60 800

5

10

15b3 vs. t

t

b3

0 20 40 60 800

5

10

15b4 vs. t

t

b4

0 20 40 60 800

5

10

15b5 vs. t

t

b5

0 20 40 60 800

5

10

15b6 vs. t

t

b6

Ground'Truth' Observability'Maximizing'Es:ma:on' Standard'Es:ma:on'

Fig. 2: Damping coefficient (bS) estimation results for for asix-link series-elastic actuated serial-chain manipulator.

positions θµ. We then use this value to update the motorreference trajectory online by taking a single fixed-lengthstep in the direction opposite that of the condition numbergradient, where the gradient is numerically calculated. Thestep length is set conservatively to ensure descent. Moreclearly, defining σS as the local estimation number associatedwith the spring parameters, θdµ as the updated motor referencetrajectory, and α ∈ R+ as the step length,

θdµ = θdµ + α∇θµσS . (31)

The updated motor control law thus becomes

u = kP (θdµ − θµ). (32)

VI. RESULTS

We demonstrate the use of the observability maximiza-tion approach discussed in Section V on a full-dynamicsimulation of a six-link series-elastic actuated mechanism.Each link is configured such that it is rotated π/2 radiansrelative to the previous link. The link and motor side second-order dynamics are assumed to have the forms (24) and(25) respectively. The link masses and moments are allnormalized to one; the motor inertia is also assumed tobe one. The physical values of the spring constants anddamping coefficients are uniformly set to k = 21 and b = 12

0 20 40 60 800

5

10

15

20

k1 vs. t

t

k1

0 20 40 60 800

5

10

15

20

k2 vs. t

t

k2

0 20 40 60 800

5

10

15

20

k3 vs. t

t

k3

0 20 40 60 800

5

10

15

20

k4 vs. t

t

k4

0 20 40 60 800

5

10

15

20

k5 vs. t

t

k5

0 20 40 60 800

5

10

15

20

k6 vs. t

t

k6

Ground'Truth' Observability'Maximizing'Es:ma:on' Standard'Es:ma:on'

Fig. 3: Spring constant (kS) estimation results for for a six-link series-elastic actuated serial-chain manipulator.

respectively (represented by the black dotted lines in Figures2 and 3). The proportional gains in (32) are set to kP = 3,the unmodified reference signal is θdµ = 1.3 sin(6t), and theparameter α in (31) is set to α = 0.1.

The code for the example in this section was writtenin C++, using Eigen, and interfaced with Matlab via mexfiles. The average time to compute the full linearization forthe six-link thirty-six dimensional system was on average0.41 × 10−4 seconds. The average time to compute thesingular value decomposition of the nonlinear observabilitymatrix rank condition was 1.3×10−3 seconds. We did not useany compile-time optimization in computing these values.

The variance terms that appear in the EKF are set to σθ =σθ = σz = 0.001, where σθ corresponds to the variance inthe angle velocities, σθ the variance in the link angles, and σzthe variance associated with the measurements. The variancevalues of the spring constants and damping coefficients areassumed to be σS = 0.1. The process and measurementcovariance matrices are both assumed to be diagonal and theinitial covariance associated with the state update is set toP0 = 0.001I , where I is a 36 × 36 identity matrix. Theinitial estimated values of the spring constants and dampingcoefficients are set to k = 3 and b = 1 respectively.

The parameter-value estimate plots are shown in Figures2 and 3. Figure 2 shows the damping coefficient results

1034

0 20 40 60 80 100 120 1400

5

10

15Error vs. t

t

erro

r

Ground'Truth' Observability'Maximizing'Es:ma:on' Standard'Es:ma:on'

Fig. 4: Total (non-dimensional) average error over all springparameters, i.e., spring constants and damping coefficients.and Figure 3 the spring constant estimates. Each of theindividual plots in these figures show the estimated param-eter values versus time; the ground truth values for eachparameter are shown in as black dotted lines. The linksare numbered from proximal to distal with respect to thebase in ascending order. The plots also show a comparisonbetween the EKF used with (dashed red line) and without(solid blue line) the modified reference trajectory. Figure 3shows a dramatic improvement in terms of the time taken toconverge when using the observability maximizing criteria.Figure 2 shows comparable performance between the twoapproaches. In fact, the observability-modified convergenceresults in Figure 2 appear to have slightly worse transientresponse than the standard filtering approach. The differencein benefits between the results shown in Figures 2 and 3 canintuitively be understood by considering how we measureobservability. The fact that we use the estimation conditionnumber has the effect of normalizing the local observabilityover all the parameters simultaneously. Thus some of theparameters which are originally very observable, i.e, havehigh associated singular values in the observability rankcondition, become slightly less observable to allow otherparameters, which have low associated singular values, tobecome much more observable. The overall improvementin performance over the entire set of parameters is quitedramatic, as can be seen in Figure 4, which plots thetotal non-dimensional average error over all of the springparameters versus time; the red-dashed line and solid blueline correspond to the observability optimized and standardestimation schemes respectively.

VII. CONCLUSION

The benefit of the work presented in this paper is shown inFigure 4. We show that using the novel online calculation ofthe nonlinear estimation condition number, made possible bythe exact closed-form forward dynamics and linearization ofthe series-elastic actuated system, a dramatic improvementin terms of the time it takes to converge to the true valuesof the unknown spring parameters can clearly be seen.

There are however several directions of future work thatwe believe can further improve our results. The first is to

include a dynamic step sizing algorithm in the calculationof α in (31). Our results show that using a fixed valueof α works reasonably well, but taking larger steps alongthe gradient of the estimation condition number initially andsmaller steps eventually can potentially lead to faster risingand better settling times in the parameter estimation signals.Additionally, in this work there is an inherent tradeoffbetween the control and estimation; the fact that we changethe reference signal in the observability-modified estimationscheme makes the comparison between the modified andstandard filtering results somewhat unfair. In the future weplan to explore how the non-uniqueness of workspace controlsolutions for redundant systems can be taken advantage ofsuch that the overall control objective remains unmodifiedwhile the observability is simultaneously maximized.

REFERENCES

[1] A. M. Bloch, J. Baillieul, P. E. Crouch, and J. E. Marsden. Nonholo-nomic Mechanics and Control. Spinger, 2003.

[2] R. W. Brockett. Robotics manipulators and the product of exponentialsformula. Proc. Int. Syp. Math. Theory of Networks and Systems, 1990.

[3] D. Del Vecchio and R.M. Murray. Observability and local observerconstruction for unknown parameters in linearly and nonlinearlyparameterized systems. Proceedings of the 2003 American ControlConference, 2003., 6(1):4748–4753, 2003.

[4] R. Hermann and A J. Krener. Nonlinear controllability and observabil-ity. IEEE Transactions on Automatic Control, 22:728–740, October1977.

[5] B T. Hinson, M K. Binder, and K A. Morgansen. Path planning tooptimize observability in a planar uniform flow field. 2013 AmericanControl Conference, (1):1392–1399, June 2013.

[6] A. Isidori. Nonlinear Control Systems. Springer, London, 1995.[7] A. Jain and G. Rodriguez. Recursive linearization of manipulator

dynamics models. IEEE International Conference on Systems, Man,and Cybernetics, pages 475–480, November 1990.

[8] A J. Krener and K Ide. Measures of unobservability. Proceedingsof the 48h IEEE Conference on Decision and Control (CDC) heldjointly with 2009 28th Chinese Control Conference, pages 6401–6406,December 2009.

[9] S. Lall, J E. Marsden, and S. Glavski. A subspace approach tobalanced truncation for model reduction of nonlinear control systems.International Journal of Robust Nonlinear Control, 12:519–535, 2002.

[10] L. Ljung. Asymptotic behavior of the extended Kalman filter as pa-rameter estimator for linear systems. IEEE Transactions on AutomaticControl, 24(1):36–50, February 1979.

[11] R. M. Murray, Z. Li, and S. Sastry. A Mathematical Introduction toRobotic Manipulation. CRC Press, 1994.

[12] D K. Pai and U M. Ascher. Forward dynamics algorithms formultibody chains and contact. IEEE International Conference onRobotics and Automation, pages 857–863, April 2000.

[13] F. C. Park, J. E. Bobrow, and S. R. Ploen. A Lie group formulationof robot dynamics. The International Journal of Robotics Research,14:609–618, December 1995.

[14] G. Rodriguez and K. Keutz-Delgado. A spatial operator factorizationand inversion of the manipulator mass matrix. IEEE Transactions onRobotics and Automation, 1992.

[15] D. Rollinson and H. Choset. Design and Architecture of a SeriesElastic Snake Robot. IEEE International Conference on IntelligentRobots and Systems, 2014.

[16] S.K. Saha. A decomposition of the manipulator inertia matrix. IEEETransactions on Robotics and Automation, 13(2):301–304, April 1997.

[17] E D. Sontag. Mathematical Control Theory. Springer, New York,1998.

[18] M W. Spong. Modeling and control of elastic joint robots. Journal ofDynamic Systems Measurement and Control, 24, 1979.

1035