Nonlinear Filtering for Hybrid GPS/GSM Mobile Terminal Tracking

Algorithm 3

Modified Cubature 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 cubature points (𝑖=1,…,2𝑛_𝑥)

𝐏(𝑘∣𝑘−1)=𝐒(𝑘∣𝑘−1)𝐒^⊤√(𝑘∣𝑘−1),Ξ=𝑛_𝑥⋅[𝐈_𝑛𝑥,−𝐈_𝑛𝑥𝒳],_𝑖(𝑘∣𝑘−1)=𝐱(𝑘∣𝑘−1)+𝐒(𝑘∣𝑘−1)𝝃_𝑖.

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

𝑊^(m)_𝑖=𝑊^(c)_𝑖=1/(2𝑛_𝑥).

Evaluate the propagated cubature points (𝑖=1,…,2𝑛_𝑥)

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

Estimate the predicted measurement

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

Estimate the innovation covariance and cross-covariance matrix

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

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

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

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

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

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

where 𝝃_𝑖 denotes the 𝑖th column vector of the matrix Ξ.