Online parameter estimation techniques

CHAPTER 4: Applications on adaptive cruise control system identification

4.3 Online parameter estimation techniques

We next introduce two online methods to estimate the adaptive cruise control model parameters using velocity, space gap, and relative velocity data.

4.3.1. Recursive least-squares formulation

First we derive a RLS estimator. Unlike (4.4), the least-squares method proposed here does not require multiple starting points or repeatedly solving an ODE within each optimization run, sub- stantially reducing the runtime. We briefly derive the least-squares formulation for the ACC car- following model (4.1).

First we rewrite the continuous time ODE (4.1) in discrete-time using a forward Euler step scheme:

v_k+1 =v_k+α(s_k−τ v_k)∆T +β(u_k−v_k)∆T, (4.5) wherevk,sk andukdenote the velocity of the follower vehicle, the space gap, and the velocity of the leading vehicle at timestep k, respectively. The term ∆T is the timestep size, which is selected to correspond to the frequency at which the velocity, space gap, and relative velocity data is measured (e.g., on the order of 1/10 of a second for some sensor platforms including the experiments presented later in this work). The dynamics can be rewritten as:

v_k+1=γ1v_k+γ2s_k+γ3u_k, (4.6) with γ1 := (1−(ατ +β)∆T), γ2 := (α∆T) and γ3 := (β∆T). Note that instead of directly estimating the parameters α, β, τ, we can instead estimate γ₁, γ₂, γ₃. Except in the degenerate case when γ2 = 0, we can always uniquely determine the values of α, β, τ given a set of values for γ1, γ2, γ3.

We now demonstrate that one can recoverγ:= [γ₁, γ₂, γ₃]^T from an experimental dataset containing (vk, sk, uk) for all k ∈ {1, ..., K}, via least-squares. We expand (4.6) in time by stacking the uniformly sampled measurements to obtain:









 v₂ v3

...

v_K











=











v₁ s₁ u₁ v2 s2 u2

... ... ...

vK−1 sK−1 uK−1













 γ1

γ2

γ₃



, (4.7)

or

Y =Xγ. (4.8)

The term Y contains the values of v_k from timestep 2 to K. The term X contains measurements of vk,sk, and uk from timestep 1 toK−1in column-wise order.

Given the data matricesY andX,γhas a unique solution if and only if rank(X) =rank([X Y]) = 3. Note that this condition is not satisfied at equilibrium, where v_i = v_j = u_k for all timesteps and . In other words, using only data from equilibrium driving, it is not possible to recover

the model parameters. In non-equilibrium driving and when sensor noise is present, it is easy to generate an over determined system of equations, motivating the search for least squares solution to (4.7).

To convert the least squares problem into an online method, a recursive implementation is desired.

The least squares solution to (4.7) has an exact recursive implementation by considering thek^throw of measurements{Y_k, X_k}one row at a time. The least squares estimate of the parameter vector at timekusing all data collected from timestep 1 throughk, denotedˆγ_k, can be sequentially updated by:

ˆ

γ_k = ˆγ_k−1+P_kX_k(Y_k−γˆ_k−1X_k), (4.9) where P_k⁻¹ =Pk

i=1XiX_i^T is the cumulative outer product of Xk. Solving (4.9) requires an initial estimate of the parameters γ₀ ∼ N(ˆγ₀, P0), which are specified in the numerical experiment in Section 4.4.

4.3.2. Online joint state and parameter estimation formulation

The parameter estimation problem can also be framed as an online joint parameter and state esti- mation problem, in which model and measurement noises are explicitly considered. Such methods, if fast enough, may also be used for real-time processing of data in order to estimate the model parameters during data collection.

Problem formulation

To jointly estimate the state and model parameters, we consider an augmented state formulation in which the model parameters are added to the state vector. The model of the evolution of the augmented state is completed by assuming the parameters are constant in time.

We proceed as follows. First,θ=[α, β, τ]^T, is concatenated to the physical system statexk∈R²= [s_k, v_k]^T to form an augmented statex^a_k∈R⁵. This is written as:

x^a_k= x

θ

k

=

s v α β τT

k . (4.10)

The discrete time dynamics of the augmented system using the same discretization approach as (4.5) can be written as:

x^a_k=F_d(x^a_k−1, uk−1), (4.11) where F_d refers the system dynamics in the augmented state. The nonlinearities in the dynamics appear due to the product of augmented state variables representing the ACC model parameters and the physical states.

The augmented state dynamics (4.11) are written as:

F_d(x^a_k−1, uk−1) =













sk−1+ ∆T(uk−1−vk−1)

vk−1+ ∆T[αk−1(sk−1−τk−1vk−1) +βk−1(uk−1−vk−1)]

αk−1

βk−1

τk−1













(4.12)

Additionally, a measurement equation is written to reflect the condition that the physical states (sk, vk) may be directly measured, but we do not measure the parameters (αk, βk, τk):

y_k=Cx^a_k=

1 0 0 0 0 0 1 0 0 0

x^a_k. (4.13)

In (4.13), y_k∈R² are the measurements, and C is the measurement matrix.

Particle filter

Because the augmented state (of the nonlinear augmented system) is to be estimated, a nonlinear estimator must be considered. Here, we outline an approach to estimate the augmented state using a PF.

The filter takes in the discrete-time system that considers model and measurement noises. The state-space form is written as:

x^a_k=F_d(x^a_k−1, uk−1) +w_k

y_k =Cx^a_k+ν_k, (4.14)

wherew_k∼(0, Q)∈R⁵ andν_k∼(0, R)∈R² are independent white noise processes for the model and the measurement equations, respectively, at time k. Q∈ R^5×5 and R ∈ R^2×2 are the known process and measurement error covariance matrices.

Recall that the Bayesian state estimation method sequentially approximates the posteriorprobability density function (PDF) of the augmented state at step k given past observations, i.e., p(x^a|y_1:k). The Bayesian state estimation method can be summarized into two parts:

1. State propagation: obtain the prior distribution at k: p(x^a_k|y_1:k−1) =

Z

p(x^a_k|x^a_1:k−1)p(x^a_k−1|y_1:k−1)dx^a_k−1 (4.15) 2. State update: obtain the posterior distribution at k:

p(x^a_k|y_1:k) = p(y_k|x^a_k)p(x^a_k|y_1:k−1)

p(y_k|y_1:k−1) . (4.16)

The particle filter [51, 38], among other filtering techniques, is deployed to approximate the prior and the posterior distributions from equations (4.15) and (4.16) because of its flexibility in noise distribution and its relaxed assumption about the linearity of the (augmented) dynamics of the

Algorithm 1 Particle filter Initialize (k= 0)

Drawiparticles{x^a,(i)₀ }_i=1:Np from an initial distributionp(x^a₀). Assign equal weightsω₀⁽ⁱ⁾= 1/N_p, wherei= 1, . . . , Np, and Np is the number of particles.

fork= 1. . . T do State propagation:

x^a,(i)_k =F_d(x^a,(i)_k−1, uk−1) +w⁽ⁱ⁾_k for alli.

State update:

Assign weight: ω_k⁽ⁱ⁾:=ω_k−1⁽ⁱ⁾ p(y_k|(x^a,(i)_k )for all i. Normalize weight: ω_k⁽ⁱ⁾:=ω_k⁽ⁱ⁾/PNp

i=1ω⁽ⁱ⁾_k for alli. Resample:

Draw x^a,(i)_k with probabilityω_k⁽ⁱ⁾ for all i. end

system. PF uses weighted particles (samples) to approximate the conditional state distribution given all measurements up to the current timestep using a sequential estimation approach. Therefore, the output is a probability distribution for each parameter at each time step.

A summary of the PF is written in Algorithm 1. During implementation, it is important to monitor the effective particle size [199] to ensure a valid estimation result. For more details on the PF implementation, readers are referred to standard references such as [188].

4.3.3. Observability analysis

In this section we provide insights on the ability to estimate the ACC model parameters via an observability analysis. An observable system indicates theoretically that its initial state can be inferred from observing the outputs. For parameter estimation in the joint state-parameter form, recovering the initial state indicates identifying the non-changing parameters. In this work we consider a special case of parameter observability if the parameters are considered as constant state variables. This assumption allows us to equate the notion of observable augmented state to uniquely identifiable parameters given measurements. Given that the augmented state dynamics (4.14) are nonlinear, observability must be assessed on a linearized version of the model. This can only be done at fixed values of the augmented state, and is explained as below.

First we write the linearized state-space model of the nonlinear discrete-time system (4.11):

x^a_k =Ak−1x^a_k−1+Bk−1uk−1

y_k =Cx^a_k, (4.17)

whereA_k is the Jacobian of F_d defined in (4.11) with respect to the augmented state variables at timek,Bkis the Jacobian with respect to the control inputs at time k, andCis the time-invariant

measurement matrix as defined above in (4.13). Further, A_k can be written as

A_k= ∂F_d

∂x^a x^a∗_k ,u^∗_k













1 −∆T 0 0 0

α∆T 1−ατ∆T −β∆T (s−τ v)∆T (u−v)∆T −αv∆T

0 0 1 0 0

0 0 0 1 0

0 0 0 0 1













x^a∗_k ,u^∗_k

, (4.18)

wherex^a∗_k , u^∗_k are the state and input points about which the system is linearized andB_kis defined similarly.

We choose to analyze the system observability by computing the above partial derivatives evaluated at an equilibrium point. The condition for equilibrium reduces to zero acceleration and space gap change, i.e.:

u_k−v_k= 0

s_k−τ_kv_k= 0 . (4.19)

In addition, the system (4.11) reduces to a linear time invariant system, and A_k from (4.18) at equilibrium simplifies to:

A=













1 −∆T 0 0 0

α∆T 1−ατ∆T−β∆T 0 0 −αv∆T

0 0 1 0 0

0 0 0 1 0

0 0 0 0 1











 .

From here the observability matrix can be calculated using Aand C as follows:

O=

C, CA, CA², CA³, CA⁴T

,

whereO is the observability matrix, the rank of which is used to assess observability.

When analyzed at any equilibrium point, the resulting observability matrix has rank(O) = 3̸= 5, corresponding to a non-observable system. The corresponding null space of the observability matrix is:

null(O) =

0 0 0 1 0 0 0 1 0 0

T

, (4.20)

indicating two unidentifiable parameters, α and β at the equilibrium points. This analysis shows that for the augmented-state estimation problem there is no guarantee for exact recovery ofαandβ at equilibrium when using filtering techniques. The observability matrix derivation in this section is based on the linearized system around an equilibrium point. Therefore, it does not provide insight on parameter estimation for non-equilibrium trajectories, which we will instead explore numerically in the computational experiments in Section 4.4 using the PF described next.