Handling Data Uncertainty and Inconsistency Using Multisensor Data Fusion

Algorithm 3

The pre- and postfiltering algorithm (F-MB-F).

  Input: 𝜎₁, 𝜎₂, 𝑧₁(𝑘), 𝑧₂(𝑘), 𝑥₁(𝑘−1), 𝑥₂(𝑘−1),

      𝑥_𝑓(𝑘−1), 𝑃₁(𝑘−1), 𝑃₂(𝑘−1), 𝑃_𝑓(𝑘−1)

  Output: 𝑥_𝑓(𝑘), 𝑃_𝑓(𝑘)

1   begin

2     𝜉←𝜎₁/𝜎₂;

3    for   𝑗←1   to  2  do

4      𝑥_𝑗(𝑘) ← Call Kalman Filter Algorithm;

5     𝑥_int(𝑘)←𝑥₁(𝑘)/(1+𝜉²)+𝑥₂(𝑘)/(1+𝜉⁻²);

6    Calculate   𝑓 as in (6);

7     𝜎²_int(𝑘)←(𝜎₁⁻²𝑓⁻¹+𝜎₂⁻²𝑓⁻¹)⁻¹;

8     (𝑥_𝑓(𝑘),𝑃_𝑓(𝑘)) ← Call Kalman Filter Algorithm;