Generalized Innovation and Inference Algorithms for Hidden Mode Switched Linear Stochastic Systems with Unknown Inputs

Transcription

1 Generalized Innovation and Inference Algorithms for Hidden Mode Switched Linear Stochastic Systems with Unnown Inputs Sze Zheng Yong a Minghui Zhu b Emilio Frazzoli a Abstract In this paper, we propose inference algorithms for simultaneously estimating the mode, input and state of hidden mode switched linear stochastic systems with unnown inputs. First, we define the generalized innovation for the recently proposed optimal filter for simultaneous input and state estimation [] and show that the sequence is a Gaussian white noise. Then, we utilize this whiteness property of the generalized innovation, which reflects the estimation quality to form the lielihood function of the system model. Consequently, we employ the multiple model (MM) approach based on the lielihood function for inferring the hidden mode of switched linear stochastic systems. Algorithms for both static and dynamic MM estimation are presented and compared using a simulation example of vehicles at an intersection with switching driver intentions. I. INTRODUCTION Most autonomous systems must operate without nowledge of the intention and the decisions of other noncommunicating systems or humans. Thus, in many instances, these intention and control decisions need to be inferred from noisy measurements. This problem can be conveniently considered within the framewor of hidden mode hybrid systems (HMHS, see, e.g., [], [3] and references therein) with unnown inputs, in which the system state dynamics is described by a finite collection of functions. Each of these functions corresponds to an intention or mode of the hybrid system, where the mode is unnown or hidden and mode transitions are autonomous. In addition, by allowing unnown inputs in this framewor, both deterministic and stochastic disturbance inputs and noise can also be considered. There are a large number of applications, such as urban transportation systems, target tracing and fault detection, in which it is not realistic to assume nowledge of the mode and disturbance inputs or they are too costly to measure. Literature review. The filtering problem of hidden mode hybrid systems without unnown inputs have been extensively studied (see, e.g., [], [] and references therein), especially in the context of target tracing applications. These filtering algorithms, which use a multiple model approach, consist of three components: (i) a ban of filters for each mode, (ii) a lielihood-based approach to determine the probability of each mode, and (iii) a hypothesis management algorithm to trade off between computational cost and estimation quality. Oftentimes, the Kalman filter [6] or a variation thereof is used as the filtering algorithm, while the a S.Z Yong and E. Frazzoli are with the Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, Cambridge, MA, USA ( szyong@mit.edu, frazzoli@mit.edu). b M. Zhu is with the Department of Electrical Engineering, Pennsylvania State University, Old Main, University Par, PA 68, USA ( muz6@psu.edu). lielihood-based mode association typically uses the whiteness property of the innovation [7], [8]. On the other hand, some popular hypothesis merging algorithms include the first- and second-order generalized pseudo-bayesian (GPB and GPB) as well as the interacting multiple model (IMM) algorithms [], [9]. However, oftentimes the disturbance inputs cannot be modeled as a zero-mean, Gaussian white noise, which gives rise to a need for an extension of the existing algorithms to hidden mode hybrid systems with unnown inputs. To the best of our nowledge, this problem has not been addressed in the literature. To develop a multiple model estimation approach for switched linear stochastic systems with unnown inputs, each component of the approach has to be reinvented. A filter algorithm for stochastic systems with unnown inputs is needed for the first component; thus, a relevant set of literature is that of optimal filters that simultaneously estimate inputs and states of linear stochastic systems with unnown inputs. Research on simultaneous input and state estimation has been gaining momentum during the past years, largely stimulated by its wide applications. Notable algorithms include [], [] []. Of all these algorithms, the optimal filter in [] is in the most general form and is hence the most suitable for the problem at hand. However, the remaining two major components of the multiple model estimation approach are still lacing at present. Contributions. In this paper, we present a novel multiple model approach for simultaneous estimation of mode, input and state of switched linear stochastic systems with unnown inputs. As with multiple model estimation of systems without unnown inputs, we also present static and dynamic variants (i.e., decoupled and with cooperation between models, respectively) of the estimation algorithm. In both variants, a ban of optimal input and state filters [], one for each mode, is run in parallel. Next, we devise a lielihood-based mode association algorithm to determine the probability of each mode. This involves the definition of a generalized innovation which we prove is a Gaussian white noise. Then, we use this whiteness property to form a lielihood function, which is used in hypothesis testing. For the dynamic variant (with cooperating models), to manage the growing number of hypotheses, we employ a similar approach to the interacting multiple model estimator [9] which mixes the initial conditions based on mode transition matrix probabilities. II. MOTIVATING EXAMPLE To motivate the problem considered in this paper, we consider the scenario of vehicles crossing a -way intersection where each vehicle does not have any information

2 about the intention of the other vehicles. To simplify the problem, we consider the case with two vehicles: Vehicle A is human driven (uncontrolled) and Vehicle B is autonomous (controlled), with dynamics described by ẍ A =.ẋ A +d and ẍ B =.ẋ B + u, where x and ẋ are vehicle positions and velocities. We assume that Vehicle A approaches the intersection with a default intention, i.e., without considering the presence of Vehicle B. Then, at the intersection, the driver of Vehicle A can choose between three intentions: to continue while ignoring the other vehicle with an unnown input d (Inattentive Driver, default mode), to attempt to cause a collision (Malicious Driver), or to stop (Cautious Driver). Then, once either vehicle completes the crossing of the intersection, Vehicle A returns to the default intention. Thus, if we assume the presence of noise, this intersectioncrossing scenario is an instance of a hidden mode switched linear stochastic system with an unnown input. The intention of driver A is a hidden mode and the actual input of vehicle A is an unnown input (which is not restricted to a finite set). The objective is to simultaneously estimate the intention (mode), input and state of the vehicles for safe navigation through the intersection. III. PROBLEM STATEMENT We consider a hidden mode switched linear stochastic system with unnown inputs: (x +, q )= (A q x +B q uq +G q d q +w q, q ), x C q (x, q ) + = (x, δ q (x )), x D q y = C q x +D q uq +H q dq +v q () where x R n is the continuous system state and q {,,..., N} the hidden discrete state or mode. For each mode q, u q U q R m is the nown input, d q R p the unnown input, y R l the output, δ q ( ) the mode transition function, C q and D q are flow and jump sets, while the process noise w q R n and the measurement noise v q R l are assumed to be mutually uncorrelated, zeromean, Gaussian white random signals with nown covariance matrices, Q q = E[w q wq ] and R q = E[v q vq ], respectively. The matrices A q, B q, Gq, C q, Dq and H q are nown. x is assumed to be independent of v q and w q for all. No prior useful nowledge of the dynamics of d q is assumed (uncorrelated with {d qj j }, j, as well as {w qj j } and {vqj j }, j) and dq can be a signal of any type. Moreover, the mode jump process is assumed to be leftcontinuous and hidden mode systems refer to systems in which q is not directly measured and the mode transitions are autonomous. We assume that in each mode, the system has strong detectability, i.e., the initial condition x and the unnown input sequence {d qj j }r j= can be uniquely determined from the measured output sequence {y i } r j= of a sufficient number of observations, i.e., r r for some r N (see [, Section 3.] for necessary and sufficient conditions for this property) and the required ran condition for the existence of a stable filter [, Theorem 9] is satisfied. The assumed permutation of intentions is for illustrative purposes only and was not a result of any limitations on the proposed algorithms. The objective of this paper is to design an optimal recursive filter algorithm which simultaneously estimates the system state x, the unnown input d q and the hidden mode q based on the measurements up to time, {y, y,..., y }. IV. PRELIMINARY MATERIAL In this section, we present a brief summary of the minimum-variance unbiased filter for linear systems with unnown inputs. For detailed proof and derivation of the filter, the reader is referred to []. Moreover, we define a generalized innovation and show that it is a Gaussian white noise. These form an essential part of the multiple model estimation algorithm that we will describe in Section V. The algorithm runs a ban of N filters (one for each mode) in parallel and each of the the filter are in essence the same except for the different sets of matrices and signals {A q, B q, Cq, Dq, Gq, H q, Qq, R q, u q, d q }. Hence, to simplify notation, the conditioning on the mode q is omitted in the entire Section IV. A. Minimum-Variance Unbiased Filter As is shown in [, Section 3.], the system for each mode after a similarity transformation is given by: x + = A x + B u + G, d, + G, d, + w () z, = C, x + D, u + Σ d, + v, (3) z, = C, x + D, u + v,. () The transformation essentially decomposes the unnown input d and the measurement y each into two orthogonal components, i.e., d, R p H and d, R p p H ; as well as z, R p H and z, R l p H, where p H = ran(h ). Then, given measurements up to time, the optimal three-step recursive filter in the minimum-variance unbiased sense can be summarized as follows: Unnown Input Estimation: ˆd, = M, (z, C, ˆx D, u ) ˆd, = M, (z, C, ˆx D, u ) () ˆd = V, ˆd, + V, ˆd, Time Update: ˆx = A ˆx + B u + G, ˆd, ˆx = ˆx + G, ˆd, (6) Measurement Update: ˆx = ˆx + L Γ (z, C, ˆx D,u ) (7) where ˆx, ˆd,, ˆd, and ˆd denote the optimal estimates of x, d,, d, and d ; Γ R p R l p H is a design matrix that is chosen to project the innovation ν := z, C, ˆx D,u onto a vector of p R independent random variables, while L R n p R, M, R p H p H and M, R (p p H ) (l p H ) are filter gain matrices that minimize the state and input error covariances. For the sae of completeness, the optimal input and state filter in [] is reproduced in Algorithm. Due to space limitation, a technical characterization of the inference algorithm will be presented in an upcoming companion paper [3].

3 Algorithm Opt-Filter (q, ˆx,q, ˆd,q,, P x,,q, P d,,q, ) [superscript q omitted in the following] Estimation of d, and d :  = A G, M, C, ; : ˆQ = G, M, R, M, G, + Q ; x, 3: P =  P  + ˆQ ; : R, = C, P C, + R, ; : P, d = (G, C, R, C,G, ) ; 6: M, = P, G d, C, R, ; 7: ˆx = A ˆx + B u + G, ˆd, ; 8: ˆd, = M, (z, C, ˆx D, u ); 9: ˆd = V, ˆd, + V, ˆd, ; : P, d = M, C, P x, A C,M, : P d = V [ P d,, P d,, G, C,M ],; P, d P, d P, d V ; Time update : ˆx = ˆx + G, ˆd, ; 3: P x = G, M, R, M,G, +(I G, M, C, ) P (I G, M, C, ) ; : R, = C, P x C, + R, C, G, M, R, R, M,G, C,; Measurement update : L = (P x C, G, M, R, ) R, ; 6: ˆx = ˆx + L (z, C, ˆx D, u ); 7: P x = (I L C, )G, M, R, L + L R, M,G, (I L C, ) +(I L C, )P (I x L C, ) + L R, L ; Estimation of d, 8: R, = C, P C x, + R, ; 9: M, = Σ ; : P, d = M, R, M,; : ˆd, = M, (z, C, ˆx D, u ); B. Properties of the Generalized Innovation Sequence In Kalman filtering, the innovation reflects the difference between the measured output at time and the optimal output forecast based on information available prior to time. The a posteriori (updated) state estimate is then a linear combination of the a priori (predicted) estimate and the weighted innovation. In the same spirit, we generalize this notion of innovation to linear systems with unnown inputs by defining a generalized innovation given by: ν := Γ (z, C, ˆx D,u ) := Γ ν (8) = Γ (I C, G, M, )(z, C, ˆx D, u ) which, similar to the conventional innovation, is weighted by L and combined with the predicted state estimate ˆx to obtain the updated state estimate ˆx as seen in (7). This definition differs from the conventional innovation in that the generalized innovation uses a subset of the measured outputs, i.e. z,. In addition, the matrix Γ is any matrix whose rows are independent of each other and are in the range space of E[ν ν ] that removes dependent components of ν (a consequence of [, Lemma 7]), which further lowers the dimension of the generalized innovation. An intuition for this is that the information contained in the unused subset is already exhausted for estimating the unnown inputs. Moreover, the optimal output forecast that is implied in (8) is a function of ˆx which contains information from the measurement at time. Nonetheless, it is clear from (8) that when there are no unnown inputs, both C, and G, are empty and Γ can be chosen to be the identity matrix, in which case the definitions of generalized innovation and (conventional) innovation coincide. In the following theorem, we establish that the generalized innovation, lie the innovation, is a Gaussian white noise. Theorem. The generalized innovation, ν given in (8) is a Gaussian white noise with zero mean and a variance of S = Γ (I C, G, M, ) R, (I C, G, M, ) Γ. Proof. Substituting () into (8), we have ν = Γ (C, x + v,). (9) Since E[ ˆx ] = and E[v,] = for all as is proven [, Lemma 3], it follows that the generalized innovation has zero mean, i.e., E[ν ] =, with covariance E[ν ν j ] = E[Γ (C, ˆx + v, )(C,j ˆx j j + v,j ) Γ j ]. We first show that the above covariance is zero when j. Without loss of generality, we assume that > j. In this case, from the properties of the filter, we have E[v, x j j ] = E[v, v,j ] =, thus the covariance reduces to E[ν νj ] = Γ C, (E[ ˆx ˆx j j ]C, + E[ ˆx v,j])γ j. () Next, to evaluate E[ ˆx ˆx j j ] and E[ ˆx v,j ], we first evaluate the a priori estimation error: ˆx + + = x + ˆx + + = A (I L Γ C, )ˆx + (I G,M,+ C,+ )w G, M,+ v,+ + G, M,+ C,+ G, M, v, A L Γ v, := Φ x + v, () where Φ and v are defined by the above while A := (I G, M,+ C,+ ) and  := A G, M, C,. With the state transition matrix of the error system given by { Φ Φ Φ j =... Φ j = Φ j+ Φ j, > j I, = j, the state estimate error is given by x = Φ j x j j + l=j Φ l+v l. () Thus, from (), we obtain E[v l x j j ] = and E[v l v,j ] = when l > j (i.e., future noise is uncorrelated with current estimate error and current noise) while when l = j, E[v j x j j ] = A jl j Γ j R,j M,j G,j and E[v j v,j ] = A j L j Γ j R,j. With this and from (), we can evaluate E[ x x j j ], E[ x v,j ] and E[ν νj ] as follows: E[ x x j j ] = Φ j+(φ j Pj j x + A jl j Γ j R,j M,j G,j ) E[ x v,j ] = Φ j+(φ j G,j M,j R,j +A j L j Γ j R,j ) E[ν νj ] = Γ C, Φ j+ (A j L j Γ j R,j M,j G,j C,j +Φ j Pj j xc,j Φ jg,j M,j R,j A j L j Γ j R,j )Γ j = Γ C, Φ j+ A j (Pj j xc,j G,j M,j R,j L j Γ j R,j )Γ j =, (3) where R,j = C,jP x j j C,j + R,j R,j M,j G,j C,j

4 C,j G,j M,jR,j and for the final equality, we substituted the filter gain from [, Section.]: L j = (P x j j C,j G,j M,j R,j )Γ j (Γ j R,j Γ j ). Finally, for j =, we can find S := E[ν ν ] as S = Γ (C, P x C, C,G, M, R, R, M, G, C, + R,)Γ = Γ (I C, G, M, ) R, (I C, G, M, ) Γ = Γ R, Γ. Furthermore, from () and (9), since we assumed that w and v for all and x are Gaussian, the generalized innovation ν is a linear combination of Gaussian random variables and is thus itself Gaussian. Therefore, we have shown that ν is a Gaussian white noise with zero mean and covariance S. Moreover, S is positive definite since Γ is chosen such that S is invertible [, Section.]. Remar. The whiteness property of the generalized innovation provides an alternative approach to derive the filter gain L in [] as can be seen by setting (3) to zero. C. Lielihood Function Since the generalized innovation ν is a Gaussian white noise with zero mean and covariance S (Theorem ), the conditional probability density function of ν given all measurements prior to time, Z, is given by f ν Z (ν Z ) = exp( ν S ν /) (π) p R/ S / = exp( ν Γ (Γ R, Γ ) Γ ν () /) (π) p R/ S / where p R := ran( R, ), ν := z, C, ˆx D, u, ν = (I C, G, M, ) ν, Γ := Γ (I C, G, M, ) and we have applied the identity R, := E[ν ν ] = (I C,G, M, ) R, (I C, G, M, ) which results from the idempotence of (I C, G, M, ) [, cf. Lemma 7]. Furthermore, the idempotence of (I C, G, M, ) implies that, similar to [, Lemma 7], we can apply [, Fact 3..9 and Proposition.6.3] to obtain p R := ran( R, ) = l p l p H with equality only when p = p H, i.e., H has full ran. Next, we note that if Γ is chosen as a matrix with orthonormal rows, Γ (Γ R, Γ ) Γ is the generalized inverse and S the pseudo-determinant of R, [, pp. 7-8]. From the above reference, for the case p R = l p < l p H, we also see that () represents the Gaussian distribution of ν R l p H whose base measure is restricted to the p R-dimensional affine subspace where the Gaussian distribution is supported. When H has full ran, the Gaussian distribution is fully supported in R l p and no restriction is necessary. Therefore, with the above base measure, we obtain the conditional probability density function of z, conditioned on the system mode, q, and all prior measurements, Z, which we define as the lielihood function at time : L(q z, ) := f z, Z,q (z, Z, q ) = f ν Z,q (ν Z, q ) = N (ν q ;, Sq ). () As proposed in [, Section.3], we can choose Γ = [ ] Ip R Ũ ˆR where Ũ matrix is obtained from the singular value decomposition of ˆR C G,. A second choice bypasses the explicit expression of Γ with the use of Moore-Penrose pseudoinverse ( ) and pseudodeterminant ( + ), i.e., Γ (Γ R, Γ ) Γ = R, and S = R, +. V. MULTIPLE MODEL APPROACH From the perspective of a hybrid system, the multiple model (MM) approach implements a ban of filters in parallel, with each corresponding to a system mode. The objective is then to decide which model/mode is the best representation of the current system mode as well as to estimate the state of the system based on this decision. In a nutshell, a Bayesian framewor is used to compute the mode probabilities µ := P (q Z ) given all measurements up to time. Using Bayes rule, we can recursively find the mode probability µ j := P (q = j Z ) at step for each mode j, given Z = {z,i, z,i } i= as µ j = P (q = j z,, z,, Z ) = P (q = j z,, Z ) = f z, q,z (z, q =j,z )P (q =j Z ) N l= f z, q,z (z, q =l,z )P (q =l Z ) = N (νj ;,Sj )P (q =j Z ) N l= N (νl ;,Sl )P (q =l Z ), (6) where we have substituted () for the final equality and assumed that the probability of q = j is independent of the measurement z,. The rationale is that since we have no nowledge about d, and the d, signal can be of any type, the measurement z, provides no useful information about the lielihood of the system mode (cf. (3)). This section presents two types of multiple model estimators static and dynamic. The static MM estimator assumes that the system mode remains constant/static, with a heuristic modification to eep the mode probabilities nonzero, while the dynamic MM estimator captures the modeswitching phenomenon by assuming that a Marov process can describe the mode jump process. In both cases, we assume that the prior mode probabilities are given P (q = j Z ) = µ j,,,..., N, (7) where Z is the prior information at time = and N j= µj =, while P (q = j Z ) in (6) differs for both estimator variants. Both multiple model estimator variants have a fixed number of models. For better performance, modifications of the algorithms in this paper could be carried out to allow for a variable structure (cf. [6] for a discussion on model selection and implementation details). A. MM Estimation: Static Variant The static MM estimator (Algorithm ) implements a ban of N mode-conditioned simultaneous input and state filters (described in Section IV-A) in parallel with the assumption that the true system mode is fixed. As such, the state estimates of other potentially mismatched models will not be beneficial and thus, the ban of filters is run independently from each other. However, in order to apply the static MM estimator to the switched linear systems, some heuristic

5 Algorithm Static-MM-Estimator ( ) : Initialize for all j {,,..., N}: ˆx j ; µj ; ˆd j, = Σj (z j, Cj,ˆxj Dj, u); P d,j, = Σj (C j, P x,j, + Rj, )Σj ; Cj : for = to K do 3: for j = to N do Mode-Matched Filtering : Run Opt-Filter(j,ˆx j, ˆdj,, P x,j f) : ν j := zj, Cj, ˆxj Dj, u ; 6: L(j z j, )= (π) pj R/ Rj,, / + exp, P d,j ( νj 7: end for 8: for j = to N do Mode Probability Update (small ɛ > ) 9: µ j = max{l(j zj, )µj, ɛ}; : end for : for j = to N do Mode Probability Update (normalization) : µ j = µj Nl= ; µ l Output 3: Compute (8); : end for : end for j, R, νj, ); modifications of the static MM estimator are necessary. Firstly, to eep all modes alive such that they can be activated when appropriate, an artificial lower bound needs to be imposed on the mode probabilities. Moreover, to deal with unacceptable growth of estimate errors of mismatched filters, reinitialization of the filters may be needed, oftentimes with estimates from the most probable mode. For each filter matched to mode j, the posterior mode probabilities can be computed with (6) where P (q = j Z ) = P (q = j Z ) = µ j, since the model is fixed. Then, these posterior mode probabilities are used to determine the most probable mode at each time and the associated state and input estimates and covariances as: j = arg max µ j ˆx = ˆx j, ˆd = ˆd j, P x = P x,j, P d = P d,j. ) ; (8) Remar. Alternatively, the output of the filter can be computed via combination of the mode-conditioned estimates: ˆx P x P d = N j= µj ˆxj, ˆd = N j= µj ˆd j, = N j= µj [(ˆxj ˆx )(ˆx j ˆx ) + P x,j ] = N j= µj [( ˆd j ˆd )( ˆd j ˆd ) + P d,j ] However, in this case, the estimate ˆd was observed (in simulation) to deviate significantly from the true value. This is consistent with the fact that with a mismatched model, the estimate of ˆdj can be arbitrarily large. Hence, this choice of filter output for ˆd is not recommended. B. MM Estimation: Dynamic Variant In contrast with the static MM estimator, the dynamic variant (Algorithm 3) assumes that the true mode switches in a Marovian manner with nown (and possibly state dependent) transition probabilities P (q = j q = i, x ) = p ij (x ), i, j,..., N. In fact, the mode transition probabilities can serve as estimator design parameters (cf. []). Therefore, the dynamic MM estimator design is more flexible and have the ability to integrate prior information of the mode switching process into the estimator. For conciseness and without loss of generality, we shall assume that the state transition probabilities are state independent, i.e., p ij (x ) = p ij. The incorporation of the state dependency for stochastic guard conditions is rather straightforward, albeit lengthy and interested readers are referred to [7] for details and examples. With the Marovian setting, the mode can change at each time step. As a result, the number of hypotheses (mode history) grows exponential with time. Therefore, an optimal multiple model filter is computationally intractable. We thus resort to suboptimal filters that manage the hypotheses in an efficient way. The simplest technique is hypothesis pruning in which a finite number of most liely hypotheses are ept, whereas the hypothesis merging approach eeps only last few of the mode histories, and combine hypotheses that differ in earlier steps (cf. [] for approaches designed for switched linear systems without unnown inputs). In the following, we propose a recursive algorithm similar to the interactive multiple model (IMM) algorithm [] which maintains N number of estimates and N number of filters at each time. Each iteration/cycle of the algorithm consists of three major components initial condition mixing, mode-matched filtering and mode probability update. In the initial condition mixing step, for each input and state filter matched to mode j at time, we compute the probability that the system was in mode i at time conditioned on Z : µ i j := P (q = i q = j, Z ) = P (q =j q =i,z )P (q =i Z ) N l= P (q =j q =l,z )P (q =l Z ) = p ijµ i P (q =j Z ) = pijµi N. l= p ljµ l (9) Then, with this mixing probabilities µ i j for all i = {,..., N}, we mix the initial conditions for the filter matched to q = j for all j = {,..., N} according to ˆx,j = N i= µi j ˆxi () ˆd,j, = N i= µi j ˆd i, () P x,,j = N i= µi j [(ˆxi ˆx,j ) (ˆx i ˆx,j ) + P x,i,j ˆd P d,,j, = N i= µi j [( ˆd i, ( ˆd i, ], ) ˆd,j, ) + P d,i, ] () Note that there is no mixing of the ˆd, and its corresponding covariances because they are computed for a previous step and are not initial conditions for the ban of filters. Next, in the mode-matched filtering step, a ban of N simultaneous input and state filter (described in Section IV-A) is run in parallel using the mixed initial conditions computed in (), () and (). In addition, the lielihood function L(q = j z, ) corresponding to each filter matched to mode

6 Algorithm 3 Dynamic-MM-Estimator ( ) : Initialize for all j {,,..., N}: ˆx j ; µj ; ˆd j, = Σj (z j, Cj,ˆxj Dj, u); P d,j, = Σj (C j, P x,j Cj, + Rj, )Σj ; : for = to K do 3: for j = to N do Initial Condition Mixing : p j = N l= p ljµ l ; : for i = to N do 6: µ i j = p ij µ i p j 7: end for 8: Compute (), () and (); Mode-Matched Filtering ; 9: Run Opt-Filter(j,ˆx,j, ˆd,j,, P x,,j, P d,,j : ν j := zj, Cj, ˆxj Dj, u ; : L(j z j, )= (π) pj R/ Rj,, / + : end for 3: for j = to N do Mode Probability Update : µ j = L(j zj, )pj Nl= ; L(j z, l )pl Output : Compute (8); 6: end for 7: end for exp ( νj j, R, νj, ); j is obtained by (). Finally, in the mode probability update step, the posterior probability of mode j given measurements up to time, µ j := P (q = j Z ), can be found by substituting P (q = j Z ) = N i= p ijµ i from (9) into (6). For output purposes only (not a major step in the algorithm), the combined estimates and covariances can be computed as given in (8) (cf. Remar ). VI. SIMULATION EXAMPLE We return to the motivating example in Section II of two vehicles crossing an intersection. Using the hidden mode system model with state x = [ ] x A, ẋ A, x B, ẋ B, each intention corresponds to a mode q {I, M, C} with the following set of parameters and inputs: Inattentive Driver (), with an unnown time-varying d (uncorrelated with x B and ẋ B, otherwise unrestricted): A I c =., BI c =, GI c =,. Cc I =, DI c =, HI c =.. Malicious Driver (), i.e., with d = K p (x B x A ) + K d (ẋ B ẋ A ) where K p = and K d = : A M c = K p. K d K p K d, HI c =. Bc M = Bc I, G M c = G I c, Cc M = Cc I, Dc M = Dc I. Cautious Driver (), i.e., with d = K p x A K d ẋ A ) ; where K p = and K d = : A M c = K p. K d, HI c =. Bc M = Bc I, G M c = G I c, Cc M = Cc I, Dc M = Dc I. Furthermore, the velocity measurement of the vehicle is corrupted by an unnown time-varying bias d. Thus, the switched linear system is described by ẋ = A q cx+b q c u+g q cd+w q, y = C q c x+d q cu+h q c d+v q, where d = [ ], d d the intensities of the zero mean, white Gaussian noises, w = [ ] w w and v, are: Q c =.6 ; R c = Since the proposed filter is for discrete-time systems, we employ a common conversion algorithm to convert the continuous dynamics to a discrete equivalent model with sample time t =.s, assuming zero-order hold for the nown and unnown inputs, u and d. From Figure, we observe that both the static and dynamic MM estimators were successful at inferring the hidden modes of the system in the cases when the vehicle remains in the Inattentive mode, or switches modes according to I M I or I C I. The performance of the static MM estimator is slightly worse than the dynamic variant, as can be seen in Figure (c). On the other hand, the changes in the mode probability estimate of the dynamic MM estimator are quicer which could be interpreted as having a higher sensitivity to mode changes. Taing a closer loo at the I M I scenario (the others are omitted due to space limitations) depicted in Figures a and b, we observe that both variants of the MM estimators performed satisfactorily in the estimation of states and unnown inputs. Similar to the observation of the mode probabilities, we note that the estimates of the static MM estimator (Figure a) are slightly inferior to that of the dynamic variant (Figure b). As aforementioned, this is because the dynamic MM estimator allows for mode transitions through a Marovian jump process where the transition matrix can be used as a design tool or incorporate prior nowledge about the mode switching process. In this example, the transition.7.. matrix is chosen as P T = VII. CONCLUSION This paper presented multiple model estimation algorithms for simultaneously estimating the mode, input and state of hidden mode switched linear stochastic systems with unnown inputs. We defined the notion of a generalized innovation sequence, which we then show to be a Gaussian white noise. Next, we exploited the whiteness property of the generalized innovations to form a lielihood function for determining mode probabilities. Simulation results for

7 Static MM Estimator (I I I) Dynamic MM Estimator (I I I) xa xb d Dynamic MM Estimator (I M I) Static MM Estimator (I M I) 3 6 (a) Vehicle remain in I mode. (b) Vehicle switches intentions I M I. (c) Vehicle switches intentions I C I. Fig. : Mode probabilities for each mode with static (top) and dynamic (bottom) MM estimators. x m A x A ˆx A 6 6 x m B x B ˆx B 6 d ˆd 6 xa xb d 6 ẋa ẋb d ẋ m A ẋ A ˆxA 6 ẋ m B ẋ B ˆxB 6 d ˆd (a) With the static MM estimator. x m A x A ˆx A 6 6 x m B x B ˆx B 6 d ˆd 6 ẋa ẋb d 6 ẋ m A ẋ A ˆxA 6 ẋ m B ẋ B ˆxB 6 d ˆd 6 (b) With the dynamic MM estimator. Fig. : Measured (superscript m, unfiltered), actual and estimated states and unnown inputs for the I M I case. vehicles at an intersection with switching driver intentions demonstrated the effectiveness of the proposed algorithms. Static MM Estimator (I C I) Dynamic MM Estimator (I C I) ACKNOWLEDGMENTS The wor presented in this paper was supported by the National Science Foundation, grant #398. M. Zhu is partially supported by ARO W9NF-3-- (MURI). REFERENCES [] S.Z. Yong, M. Zhu, and E. Frazzoli. A unified filter for simultaneous input and state estimation of linear discrete-time stochastic systems. Automatica,. Provisionally accepted. Available from: http: //arxiv.org/abs/ [] R. Verma and D. Del Vecchio. Safety control of hidden mode hybrid systems. IEEE Transactions on Automatic Control, 7():6 77,. [3] S.Z. Yong and E. Frazzoli. Hidden mode tracing control for a class of hybrid systems. In Proceedings of the American Control Conference, pages 73 7, June 3. [] Y. Bar-Shalom, T. Kirubarajan, and X.R. Li. Estimation with Applications to Tracing and Navigation. John Wiley & Sons, Inc., New Yor, NY, USA,. [] E. Mazor, A. Averbuch, Y. Bar-Shalom, and J. Dayan. Interacting multiple model methods in target tracing: a survey. IEEE Transactions on Aerospace and Electronic Systems, 3():3 3, Jan 998. [6] R.E. Kalman. A new approach to linear filtering and prediction problems. Transactions of the ASME Journal of Basic Engineering, 8(Series D):3, 96. [7] T. Kailath. An innovations approach to least-squares estimation part i: Linear filtering in additive white noise. Automatic Control, IEEE Transactions on, 3(6):66 6, Dec 968. [8] P.D. Hanlon and P.S. Maybec. Multiple-model adaptive estimation using a residual correlation alman filter ban. Aerospace and Electronic Systems, IEEE Transactions on, 36():393 6, Apr. [9] H.A. Blom and Y. Bar-Shalom. The interacting multiple model algorithm for systems with Marovian switching coefficients. IEEE Transactions on Automatic Control, 33(8):78 783, August 988. [] S. Gillijns and B. De Moor. Unbiased minimum-variance input and state estimation for linear discrete-time systems. Automatica, 3(): 6, January 7. [] S. Gillijns and B. De Moor. Unbiased minimum-variance input and state estimation for linear discrete-time systems with direct feedthrough. Automatica, 3():93 937, 7. [] S.Z. Yong, M. Zhu, and E. Frazzoli. Simultaneous input and state estimation for linear discrete-time stochastic systems with direct feedthrough. In Conference on Decision and Control (CDC), pages , 3. [3] S.Z. Yong, M. Zhu, and E. Frazzoli. Resilient state estimation against switching attacs on stochastic cyber-physical systems. In Proceedings of the American Control Conference,. Submitted. [] D.S. Bernstein. Matrix Mathematics: Theory, Facts, and Formulas (Second Edition). Princeton University Press, 9. [] C.R. Rao. Linear statistical inference and its applications. Wiley, 973. [6] X.R. Li and Y. Bar-Shalom. Multiple-model estimation with variable structure. IEEE Transactions on Automatic Control, ():78 93, 996. [7] C.E. Seah and I. Hwang. State estimation for stochastic linear hybrid systems with continuous-state-dependent transitions: an IMM approach. IEEE Transactions on Aerospace and Electronic Systems, ():376 39, 9.