Define the system
xk+1=Fkxk+Gkwk+Γkuk(1)zk=H′kxk+vk(2) {uk} is known, x0∼(ˉx0,P0) and {wk},{vk} are random sequences with
[wkvk]∼([00],[QkSkS′kRk]) with [w′kv′k]′ independent of other vectors indexed by l≠k and x0
One step predictor estimate
First we seek a recursive equation for ˆxk|k−1=E[xk|Zk−1]=E[xk|˜Zk−1] Define ˜xk=xk−ˆxk|k−1, note that {˜xk} is not an innovations sequence. Because of the independence of the innovations we have
E[xk+1|˜Zk]=E[xk+1|˜zk]+E[xk+1|˜Zk−1]−ˉxk+1
Where ˉxk=E[xk]. Recall
E[xk+1|˜zk]=ˉxk+1+cov(xk+1,˜zk)cov−1(˜zk,˜zk)˜zk Define the error covariance matrix Σk|k−1=E[˜xk˜x′k] Then
cov(xk+1,˜zk)=cov(Fkxk+Gkwk+Γkuk,H′k˜xk+vk)=E[(Fkxk+Gkwk−Fkˉxk)(˜x′kHk+v′k)]=E[Fkxk˜x′kHk]+GkSk=Fk[E(ˆxk|k−1˜x′k)+E(˜xk˜x′k)]Hk+GkSk=FkΣk|k−1Hk+GkSk Observe that ˆzk|k−1=H′ˆxk|k−1 and subtracting from (2) gives ˜zk=H′k˜xk+vk. Also note that E[ˆxk˜x′k]=0. Next
cov(˜zk,˜zk)=cov(H′k˜xk+vk,H′k˜xk+vk)=H′kΣk|k−1Hk+Rk=Ωk We also have
E[xk+1|˜Zk−1]=E[Fkxk+Gkwk+Γkuk|˜Zk−1]=FkE[xk|˜Zk=1]+Γkuk=Fkˆxk|k−1+Γkuk Collecting all terms above, the recursion becomes
ˆxk+1|k=Fkˆxk|k−1+Γkuk+Kk(zk−H′kˆxk|k−1)(9) with Kk=(FkΣk|k−1Hk+GkSk)Ω−1k
The recursion of the error covariance is developed next. From (1),(9), using the identity ˜xk+1=xk+1−ˆxk+1|k and expanding zk using (2).
˜xk+1=(Fk−KkH′k)˜xk+Gkwk−Kkvk Since ˜xk and [w′kv′k]′ are independent and zero mean, we get
E[˜xk+1˜x′k+1]=(Fk−KkH′k)E(˜xk˜x′k)(Fk−KkH′k)′×[Gk−Kk][QkSkS′kRk][G′k−K′k] or
Σk+1|k=(Fk−KkH′k)Σk|k−1(Fk−KkH′k)′+GkQkG′k+KkRkK′k−GkSkK′k−KkS′kG′k
Filtered estimates
Defined in terms of ˆxk+1|k and zk+1
ˆxk+1|k+1=E[xk+1|˜Zk+1]=E[xk+1|˜zk+1]+E[xk+1|˜Zk]−ˉxk+1=ˉxk+1+cov(xk+1,˜zk+1)cov−1(˜zk+1,˜zk+1)˜zk+1+ˆxk+1|k−ˉxk+1
Now cov(xk+1,˜zk+1)=E[(˜xk+1+ˆxk+1|k−ˉxk+1)(˜xk+1Hk+1+vk+1)]=E[˜xk+1˜x′k+1]Hk+1=Σk+1|kHk+1
From early results, we have cov(˜zk+1,˜zk+1)=H′k+1Σk+1|kHk+1+Rk+1=Ωk+1 The measurement-update (filtered estimate) is
ˆxk+1|k+1=ˆxk+1|k+Σk+1|kHk+1Ω−1k+1(zk+1−H′k+1ˆxk+1|k)(6)
Define the uncorrelated input noise ˜wk=wk−ˆwk=wk−SkR−1kvk such that
[˜wkvk]∼([00],[Qk−SkR−1kS′k00Rk])
then we have
xk+1=Fkxk+Gk˜wk+GkSkR−1kvk+Γkuk=(Fk−GkSkR−1kH′k)xk+Gk˜wk+Γkuk+GkSkR−1kzk using the fact vk=zk−H′kxk .Noting that E[˜wkv′k]=0, the time update equation becomes
ˆxk+1|k=(Fk−GkSkR−1kH′k)ˆxk|k+Γkuk+GkSkR−1kzk(5)
Error covariance for filtered estimates
The error covariance is
Σk|k=E[(xk−ˆxk|k)(xk−ˆxk|k)′]
From (6) we have
(xk+1−ˆxk+1|k+1)+Σk+1|kHk+1Ω−1k+1˜zk+1=xk+1−ˆxk+1|k
By the orthogonality principle, xk+1−ˆxk+1|k+1 is orthogonal to ˜zk+1. Therefore,
Σk+1|k+1+Σk+1|kHk+1Ω−1k+1H′k+1Σk+1|k=Σk+1|k or
Σk+1|k+1=Σk+1|k−Σk+1|kHk+1Ω−1k+1H′k+1Σk+1|k
Lastly, we obtain the time-update error covariance, subtracting (5) from (1)
xk+1−ˆxk+1|k=(Fk−GkSkR−1kH′k)(xk−ˆxk|k)+Gw˜wk and using the orthogonality of ˜wk and xk−ˆxk|k, we obtain
Σk+1|k=(Fk−GkSkR−1kH′k)Σk|k(Fk−GkSkR−1kH′k)′+Gk(Qk−SkR−1kS′k)Gk
Summary
Measurement update
ˆxk+1|k+1=ˆxk+1|kH′k+1Ω−1k+1(zk+1−H′k+1ˆxk+1|k)Σk+1|k+1=Σk+1|k−Σk+1|kHk+1Ω−1k+1H′k+1Σk+1|kΩk+1=H′k+1Σk+1|kHk+1+Rk+1
Time update
ˆxk+1|k=(Fk−GkSkR−1kH′k)ˆxk|k+Γkuk+GkSkR−1kzkΣk+1|k=(Fk−GkSkR−1kH′k)Σk|k(Fk−GkSkR−1kH′k)′+Gk(Qk−SkR−1kS′k)G′k
Time update with Sk=0
ˆxk+1|k=Fkˆxk|k+ΓkukΣk+1|k=FkΣk|kF′k+GkQkG′k
Combined update with Sk=0 for filtered state:
ˆxk+1|k+1=Fkˆxk|k+Lk+1(zk+1−H′k+1Fkˆxk|k−H′k+1Γkuk)Lk+1=Σk+1|kHk+1Ω−1k+1Ωk+1=H′k+1Σk+1|kHk+1+Rk+1
No comments:
Post a Comment