Nonlinear Filtering for Hybrid GPS/GSM Mobile Terminal Tracking

Algorithm 2

Rao-Blackwellized Unscented Kalman Filter.

Initialization

𝐱(0∣0)=𝔼{𝐱(0)}𝐏(0∣0)=𝔼{(𝐱(0)−𝐱(0∣0))(𝐱(0)−𝐱(0∣0))^⊤}

Time Update

Estimate the predicted state vector and error covariance matrix

𝐱(𝑘∣𝑘−1)=Φ𝐱(𝑘−1∣𝑘−1)𝐏(𝑘∣𝑘−1)=Φ𝐏(𝑘−1∣𝑘−1)Φ^⊤+Γ𝐐Γ^⊤

Measurement Update

Evaluate the sigma points (𝑖=1,…,𝑛_𝑥)

𝒳₀𝒳(𝑘∣𝑘−1)=𝐱(𝑘∣𝑘−1),_𝑖(𝑘∣𝑘−1)=𝐱(𝑘∣𝑘−1)+((𝑛_𝑥+𝜆)𝐏(𝑘∣𝑘−1))_𝑖,𝒳_{𝑛𝑥+𝑖}(𝑘∣𝑘−1)=𝐱(𝑘∣𝑘−1)−((𝑛_𝑥+𝜆)𝐏(𝑘∣𝑘−1))_𝑖.

Evaluate the weights (𝑖=1,…,2𝑛_𝑥)

𝑊^(m)₀=𝜆𝑛_𝑥+𝜆,𝑊^(c)₀=𝜆𝑛_𝑥+𝜆+(1−𝛼²𝑊+𝛽),_(m)𝑖=𝑊^(c)_𝑖=12(𝑛_𝑥.+𝜆)

Evaluate the propagated sigma points (𝑖=0,…,2𝑛_𝑥)

𝒴_𝑖(𝑘∣𝑘−1)=𝐡(𝒳_𝑖(𝑘∣𝑘−1)).

Estimate the predicted measurement

𝐲(𝑘∣𝑘−1)=∑^2𝑛𝑥_𝑖=0𝑊^(m)_𝑖𝒴_𝑖(𝑘∣𝑘−1).

Estimate the innovation covariance and cross-covariance matrix (𝑖=0,…,2𝑛_𝑥)

𝒳_𝑖(𝑘∣𝑘−1)=𝒳_𝑖𝒴(𝑘∣𝑘−1)−𝐱(𝑘∣𝑘−1),_𝑖(𝑘∣𝑘−1)=𝒴_𝑖𝐏(𝑘∣𝑘−1)−𝐲(𝑘∣𝑘−1),_𝐲𝐲(𝑘∣𝑘−1)=∑^2𝑛𝑥_𝑖=0𝑊^(c)_𝑖𝒴_𝑖(𝑘∣𝑘−1)𝒴^⊤_𝑖𝐏(𝑘∣𝑘−1)+𝐑,_𝐱𝐲(𝑘∣𝑘−1)=∑^2𝑛𝑥_𝑖=0𝑊^(c)_𝑖𝒳_𝑖(𝑘∣𝑘−1)𝒴^⊤_𝑖(𝑘∣𝑘−1).

Estimate the Kalman gain, updated state vector and error covariance matrix

𝐊(𝑘)=𝐏_𝐱𝐲(𝑘∣𝑘−1)𝐏⁻¹_𝐲𝐲(𝑘∣𝑘−1),

𝐱(𝑘∣𝑘)=𝐱(𝑘∣𝑘−1)+𝐊(𝑘)[𝐲(𝑘)−𝐲(𝑘∣𝑘−1)−𝝁],

𝐏(𝑘∣𝑘)=𝐏(𝑘∣𝑘−1)−𝐊(𝑘)𝐏_𝐲𝐲(𝑘∣𝑘−1)𝐊^⊤(𝑘),

where 𝑛_𝑥 is the dimension of state vector, 𝜆=𝛼²(𝑛_𝑥+𝜅)−𝑛_𝑥 is a scaling

parameter, 𝛼 determines the spread of the sigma points, 𝜅 is a secondary scaling

parameter and 𝛽 is a weight parameter. (𝐀)_𝑖 denotes the 𝑖th column of the

matrix 𝐀