Filtro de kalman en python

Implementation of Kalman Filter with Python Language
Mohamed LAARAIEDH IETR Labs, University of Rennes 1 Mohamed.laaraiedh@univ-rennes1.fr

Abstract
In this paper, we investigate the implementation of a Python code for a Kalman Filter using the Numpy package. A Kalman Filtering is carried out in two steps: Prediction and Update. Each step is investigated and coded as a function with matrixinput and output. These different functions are explained and an example of a Kalman Filter application for the localization of mobile in wireless networks is given.

I.

Introduction

Within the significant toolbox of mathematical tools that can be used for stochastic estimation from noisy sensor measurements, one of the most well-known and oftenused tools is what is known as the Kalmanfilter. The Kalman filter is named after Rudolph E. Kalman, who in 1960 published his famous paper describing a recursive solution to the discrete-data linear filtering problem [3]. The Kalman filter is essentially a set of mathematical equations that implement a predictor-corrector type estimator that is optimal in the sense that it minimizes the estimated error covariance when some presumedconditions are met. Since the time of its introduction, the Kalman filter has been the subject of extensive research and application, particularly in the area of autonomous or assisted navigation. This is likely due in large part to advances in digital computing that made the use of the filter practical, but also to the relative simplicity and robust nature of the filter itself. A complete tutorial aboutKalman filtering is given in [2].

I.1.

Mathematical Formulation of Kalman Filter

The Kalman filter addresses the general problem of trying to estimate the state x ∈ ℜ n of a discrete-time controlled process that is governed by the linear stochastic difference equation x k = Ax k −1 + Bu k + wk −1 (1) with a measurement y ∈ ℜm that is y k = Hx k + v k (2) The random variables w k and v krepresent the process and measurement noise respectively. They are assumed to be independent of each other, white, and with normal probability distributions p ( w) ≈ N (0, Q ) (3) p (v) ≈ N (0, R ) (4) -1-

In practice, the process noise covariance Q and measurement noise R covariance matrices might change with each time step or measurement, however here we assume they are constant. The n × nmatrix A relates the state at the previous time step to the state at the current step, in the absence of either a driving function or process noise. The n × 1 matrix B relates the optional control input u ∈ ℜ l to the state x. The m × n matrix H in the measurement equation relates the state to the measurement y k . For more deep investigation of the Kalman Filter you may see the reference [2].I.2.

How does Kalman Filter work?

The Kalman filter process has two steps: the prediction step, where the next state of the system is predicted given the previous measurements, and the update step, where the current state of the system is estimated given the measurement at that time step. The steps translate to equations as follows [2]:

• Prediction X k− = Ak −1 X k −1 + Bk U k
T Pk− = Ak−1 Pk −1 Ak −1 + Qk −1 • Update Vk = Yk − H k X k−

(5) (6) (7) (8) (9) (10)

S k = H k P H + Rk
T k T K k = Pk− H k S k−1

− k

X k = X + K k Vk Pk = P − K k S k K
− k T k

− k

(11)

where • X k− and Pk− are the predicted mean and covariance of the state, respectively, on the time step k before seeing the measurement. • X k and Pk are the estimated mean and covariance of the state,respectively, on time step k after seeing the measurement. • Y k is mean of the measurement on time step k . • Vk is the innovation or the measurement residual on time step k . • S k is the measurement prediction covariance on the time step k . • K k is the filter gain, which tells how much the predictions should be corrected on time step k .

I.3.

Applications of the Kalman Filter

The...