A Clustering-Based Approach for Improving the Accuracy of UWB Sensor-Based Indoor Positioning System

Algorithm 2

The Kalman filter algorithm in pseudocode.

	Input: Q, R, z, Xest, Pest
	Output: ,
	Step 1. Initialize T matrix and M matrix
	Step 2. Predict the state vector and the covariance:
	Xprd = T  Xest
	Pprd = T  Pest
	Step 3. Estimation step:
	S = M
	B = M
	Step 4. Compute the Kalman gain factor:
	klm_gain =
	Step 5. Correction based on observation:
	= Xprd + klm_gain  (z−M  Xprd)
	= Pprd−klm_gain  M  Pprd
	Step 6. Return  ,