Research Article
Research on Extended Kalman Filter and Particle Filter Combinational Algorithm in UWB and Foot-Mounted IMU Fusion Positioning
Algorithm 1
UwbDataFilter = KF(UwbData).
| Input: UwbData: the original UWB data | | Output: UwbDataFilter: UWB data after filtering | | (1) | UwbDataFilter = [] | | (2) | For j in UwbData.cols() | | (3) | EstimateCov = 0.5, MeasureCov = 0.5, Estimate = 0.0 | | (4) | For i in UwbData.rows() | | (5) | K = EstimateCov ∗ sqrt(1/(EstimateCov^2 + MeasureCov^2)) | | (6) | Estimate = Estimate + K ∗ (UwbData[j, i] − Estimate) | | (7) | EstimateCov = np.sqrt(1 − K) ∗ EstimateCov | | (8) | MeasureCov = np.sqrt(1 − K) ∗ MeasureCov | | (9) | UwbDataFilter[j, i] = Estimate | | (10) | End | | (11) | End | | (12) | Return UwbDataFilter |
|
