# kalman filter - python implementation

Started by June 8, 2010
```Hi, I try to implement kalman filter (Python 2.6), and I have a problem
with covariance matrix, which in some time start to have crazy values
(going to minus infinity) and in effect my estimations are also crazy.

For example:

observation:  [[ -0.21369917]
[  1.76860362]
[  5.57973197]
[ 12.32486812]
[ 20.49270401]
[ 31.83940345]
[ 41.51642446]]
X_estimate =  [  0.00000000e+00   1.34490511e+00   4.33627110e+00
1.26596826e+01
2.08888784e+01  -3.25942333e+02  -9.21322474e+05]

Code:

from numpy import *
from numpy.matlib import randn
from numpy.linalg import inv,det

from random import *
from numpy.random import *

def predict(x, P, A, Q, B, u):
x = dot(A,x) + dot(B, u)
P = dot(A, dot(P, A.T)) + Q
return(x,P)

def update(x, P, z, H, R):

Hx = dot(H, x)
IS = R + dot(H, dot(P, H.T))
PH = dot(P,H.T)
K= dot(PH,inv(IS))
x = x + dot(K, (z-Hx))#by miec skalar, a nie wektor 1 elementowy ...

P = P - dot(K, dot(H, P))
return (x,P)

def KalmanInit():
# intial parameters
n_iter = 7 #iteration number
#X - state vector = [teta,omega, alfa]
#teta - angle
#omega(n)= [teta(n+1)-teta(n-1)]/2  temporary freq.
#alfa(n)=(teta(n+1)-teta(n))- (teta(n)-teta(n-1))=
teta(n+1)-2teta(n)+teta(n-1) modulation factor
X=zeros((n_iter,3))
X_estimate=zeros((n_iter,3))
X_predict=zeros((n_iter,3))
teta_list=zeros(n_iter+1)
omega_list=zeros(n_iter)
alfa_list=zeros(n_iter)

for n in range(n_iter+1):
teta_list[n]=n+n*n

#temporary freq. and modulation factor - not important if H= [1 0 0]

#for n in range(1,n_iter):# dla n = 0 -> omega,alfa=0
#    omega_list[n]=(teta_list[n+1]-teta_list[n-1])/2
#    alfa_list[n]=teta_list[n+1]-2*teta_list[n]+teta_list[n-1]
for n in range(n_iter):
X[n]=teta_list[n]
#X[n]=omega_list[n]
#X[n]=alfa_list[n]

X=array(X)
A  = array([[1, 1, 0.5], [0, 1, 1], [0, 0, 1]])
H = array([1, 0, 0])
B = eye(X.shape)
#print B.shape=(3,3)
U = zeros((n_iter,X.shape))
Q = eye(X.shape)
R = eye(X.shape)
P_init  = diag((0.01, 0.01, 0.01))
Z=zeros((n_iter,1))
V = normal(0,1,n_iter)

for n in range(n_iter):
Z[n]=dot(H,X[n])+V[n]

return n_iter,X_estimate,X_predict,A,H,Q,B,U,R,P_init,Z

def ccmKalman():

n_iter,X_estimate,X_predict,A,H,Q,B,U,R,P_estimate,Z=KalmanInit()
print "observation: ", Z

for n in range(1,n_iter):
(X_predict[n],P_predict)=predict(X_estimate[n-1],P_estimate, A, Q,
B, U[n-1])
(X_estimate[n],P_estimate)=update(X_predict[n],P_predict, Z[n], H,
R)

print "X_estimate = ", X_estimate.T

ccmKalman()

Can someone help me, cos I have no idea what's wrong ?

```
```Me not sore sure wht doin'.  Mayb clr with discription of what doing. and consider
audience.  Cos no one hep if can't not understands you.

Caldin.

---
frmsrcurl: http://compgroups.net/comp.dsp/kalman-filter-python-implementation
```
```On Jun 9, 7:41&#2013266080;am, "grzegorz g."
<grzegorz.gwardys@n_o_s_p_a_m.gmail.com> wrote:
> Hi, I try to implement kalman filter (Python 2.6), and I have a problem
> with covariance matrix, which in some time start to have crazy values
> (going to minus infinity) and in effect my estimations are also crazy.
>
> For example:
>
> observation: &#2013266080;[[ -0.21369917]
> &#2013266080;[ &#2013266080;1.76860362]
> &#2013266080;[ &#2013266080;5.57973197]
> &#2013266080;[ 12.32486812]
> &#2013266080;[ 20.49270401]
> &#2013266080;[ 31.83940345]
> &#2013266080;[ 41.51642446]]
> X_estimate = &#2013266080;[ &#2013266080;0.00000000e+00 &#2013266080;
1.34490511e+00 &#2013266080; 4.33627110e+00 &#2013266080;
> 1.26596826e+01
> &#2013266080; &#2013266080;2.08888784e+01 &#2013266080;-3.25942333e+02
&#2013266080;-9.21322474e+05]
>
> Code:
>
> from numpy import *
> from numpy.matlib import randn
> from numpy.linalg import inv,det
>
> from random import *
> from numpy.random import *
>
> def predict(x, P, A, Q, B, u):
> &#2013266080; &#2013266080; x = dot(A,x) + dot(B, u) &#2013266080;
> &#2013266080; &#2013266080; P = dot(A, dot(P, A.T)) + Q
> &#2013266080; &#2013266080; return(x,P)
>
> def update(x, P, z, H, R):
>
> &#2013266080; &#2013266080; Hx = dot(H, x)
> &#2013266080; &#2013266080; IS = R + dot(H, dot(P, H.T))
> &#2013266080; &#2013266080; PH = dot(P,H.T)
> &#2013266080; &#2013266080; K= dot(PH,inv(IS))
> &#2013266080; &#2013266080; x = x + dot(K, (z-Hx))#by miec skalar, a nie wektor
1 elementowy ...
>
> &#2013266080; &#2013266080; P = P - dot(K, dot(H, P))
> &#2013266080; &#2013266080; return (x,P)
>
> def KalmanInit():
> &#2013266080; &#2013266080; # intial parameters
> &#2013266080; &#2013266080; n_iter = 7 #iteration number
> &#2013266080; &#2013266080; #X - state vector = [teta,omega, alfa]
> &#2013266080; &#2013266080; #teta - angle
> &#2013266080; &#2013266080; #omega(n)= [teta(n+1)-teta(n-1)]/2
&#2013266080;temporary freq.
> &#2013266080; &#2013266080; #alfa(n)=(teta(n+1)-teta(n))- (teta(n)-teta(n-1))=
> teta(n+1)-2teta(n)+teta(n-1) modulation factor
> &#2013266080; &#2013266080; X=zeros((n_iter,3))
> &#2013266080; &#2013266080; X_estimate=zeros((n_iter,3))
> &#2013266080; &#2013266080; X_predict=zeros((n_iter,3))
> &#2013266080; &#2013266080; teta_list=zeros(n_iter+1) &#2013266080;
> &#2013266080; &#2013266080; omega_list=zeros(n_iter) &#2013266080;
> &#2013266080; &#2013266080; alfa_list=zeros(n_iter) &#2013266080;
>
> &#2013266080; &#2013266080; for n in range(n_iter+1):
> &#2013266080; &#2013266080; &#2013266080; &#2013266080; teta_list[n]=n+n*n
>
> &#2013266080; &#2013266080; #temporary freq. and modulation factor - not important
if H= [1 0 0] &#2013266080;
>
> &#2013266080; &#2013266080; #for n in range(1,n_iter):# dla n = 0 -> omega,alfa=0
> &#2013266080; &#2013266080; # &#2013266080;
&#2013266080;omega_list[n]=(teta_list[n+1]-teta_list[n-1])/2
> &#2013266080; &#2013266080; # &#2013266080;
&#2013266080;alfa_list[n]=teta_list[n+1]-2*teta_list[n]+teta_list[n-1]
> &#2013266080; &#2013266080; for n in range(n_iter):
> &#2013266080; &#2013266080; &#2013266080; &#2013266080; X[n]=teta_list[n]
> &#2013266080; &#2013266080; &#2013266080; &#2013266080; #X[n]=omega_list[n]
> &#2013266080; &#2013266080; &#2013266080; &#2013266080; #X[n]=alfa_list[n]
>
> &#2013266080; &#2013266080; X=array(X)
> &#2013266080; &#2013266080; A &#2013266080;= array([[1, 1, 0.5], [0, 1, 1], [0, 0,
1]])
> &#2013266080; &#2013266080; H = array([1, 0, 0])
> &#2013266080; &#2013266080; B = eye(X.shape)
> &#2013266080; &#2013266080; #print B.shape=(3,3)
> &#2013266080; &#2013266080; U = zeros((n_iter,X.shape))
> &#2013266080; &#2013266080; Q = eye(X.shape)
> &#2013266080; &#2013266080; R = eye(X.shape)
> &#2013266080; &#2013266080; P_init &#2013266080;= diag((0.01, 0.01, 0.01))
&#2013266080; &#2013266080;
> &#2013266080; &#2013266080; Z=zeros((n_iter,1))
> &#2013266080; &#2013266080; V = normal(0,1,n_iter)
>
> &#2013266080; &#2013266080; for n in range(n_iter):
> &#2013266080; &#2013266080; &#2013266080; &#2013266080; Z[n]=dot(H,X[n])+V[n]
>
> &#2013266080; &#2013266080; return
n_iter,X_estimate,X_predict,A,H,Q,B,U,R,P_init,Z
>
> def ccmKalman():
>
> &#2013266080; &#2013266080;
n_iter,X_estimate,X_predict,A,H,Q,B,U,R,P_estimate,Z=KalmanInit()
> &#2013266080; &#2013266080; print "observation: ", Z
>
> &#2013266080; &#2013266080; for n in range(1,n_iter):
> &#2013266080; &#2013266080; &#2013266080; &#2013266080;
(X_predict[n],P_predict)=predict(X_estimate[n-1],P_estimate, A, Q,
> B, U[n-1])
> &#2013266080; &#2013266080; &#2013266080; &#2013266080;
(X_estimate[n],P_estimate)=update(X_predict[n],P_predict, Z[n], H,
> R)
>
> &#2013266080; &#2013266080; print "X_estimate = ", X_estimate.T
>
> ccmKalman()
>
> Can someone help me, cos I have no idea what's wrong ?

Why the Hell Python? Why not use Javascript while you're at it!
```
```On 6/8/2010 6:55 PM, HardySpicer wrote:

> On Jun 9, 7:41 am, "grzegorz g."
>
> Why the Hell Python? Why not use Javascript while you're at it!

fyi, Python is used a lot in mathematics software these days. Sage uses
Python, Numpy is in Python, as well as SciPy, and many more.  Many
people write scientific software in Python.

--Nasser

```
```On 06/08/2010 12:41 PM, grzegorz g. wrote:
> Hi, I try to implement kalman filter (Python 2.6), and I have a problem
> with covariance matrix, which in some time start to have crazy values
> (going to minus infinity) and in effect my estimations are also crazy.
>
> For example:
>
> observation:  [[ -0.21369917]
>   [  1.76860362]
>   [  5.57973197]
>   [ 12.32486812]
>   [ 20.49270401]
>   [ 31.83940345]
>   [ 41.51642446]]
> X_estimate =  [  0.00000000e+00   1.34490511e+00   4.33627110e+00
> 1.26596826e+01
>     2.08888784e+01  -3.25942333e+02  -9.21322474e+05]
>
> Code:
>
> from numpy import *
> from numpy.matlib import randn
> from numpy.linalg import inv,det
>
> from random import *
> from numpy.random import *
>
>
> def predict(x, P, A, Q, B, u):
>      x = dot(A,x) + dot(B, u)
>      P = dot(A, dot(P, A.T)) + Q
>      return(x,P)
>
> def update(x, P, z, H, R):
>
>      Hx = dot(H, x)
>      IS = R + dot(H, dot(P, H.T))
>      PH = dot(P,H.T)
>      K= dot(PH,inv(IS))
>      x = x + dot(K, (z-Hx))#by miec skalar, a nie wektor 1 elementowy ...
>
>      P = P - dot(K, dot(H, P))
>      return (x,P)
>
>
> def KalmanInit():
>      # intial parameters
>      n_iter = 7 #iteration number
>      #X - state vector = [teta,omega, alfa]
>      #teta - angle
>      #omega(n)= [teta(n+1)-teta(n-1)]/2  temporary freq.
>      #alfa(n)=(teta(n+1)-teta(n))- (teta(n)-teta(n-1))=
> teta(n+1)-2teta(n)+teta(n-1) modulation factor
>      X=zeros((n_iter,3))
>      X_estimate=zeros((n_iter,3))
>      X_predict=zeros((n_iter,3))
>      teta_list=zeros(n_iter+1)
>      omega_list=zeros(n_iter)
>      alfa_list=zeros(n_iter)
>
>      for n in range(n_iter+1):
>          teta_list[n]=n+n*n
>
>      #temporary freq. and modulation factor - not important if H= [1 0 0]
>
>      #for n in range(1,n_iter):# dla n = 0 ->  omega,alfa=0
>      #    omega_list[n]=(teta_list[n+1]-teta_list[n-1])/2
>      #    alfa_list[n]=teta_list[n+1]-2*teta_list[n]+teta_list[n-1]
>      for n in range(n_iter):
>          X[n]=teta_list[n]
>          #X[n]=omega_list[n]
>          #X[n]=alfa_list[n]
>
>      X=array(X)
>      A  = array([[1, 1, 0.5], [0, 1, 1], [0, 0, 1]])
>      H = array([1, 0, 0])
>      B = eye(X.shape)
>      #print B.shape=(3,3)
>      U = zeros((n_iter,X.shape))
>      Q = eye(X.shape)
>      R = eye(X.shape)
>      P_init  = diag((0.01, 0.01, 0.01))
>      Z=zeros((n_iter,1))
>      V = normal(0,1,n_iter)
>
>      for n in range(n_iter):
>          Z[n]=dot(H,X[n])+V[n]
>
>      return n_iter,X_estimate,X_predict,A,H,Q,B,U,R,P_init,Z
>
> def ccmKalman():
>
>      n_iter,X_estimate,X_predict,A,H,Q,B,U,R,P_estimate,Z=KalmanInit()
>      print "observation: ", Z
>
>      for n in range(1,n_iter):
>          (X_predict[n],P_predict)=predict(X_estimate[n-1],P_estimate, A, Q,
> B, U[n-1])
>          (X_estimate[n],P_estimate)=update(X_predict[n],P_predict, Z[n], H,
> R)
>
>
>      print "X_estimate = ", X_estimate.T
>
>
> ccmKalman()
>
>
>
> Can someone help me, cos I have no idea what's wrong ?

If you will endeavor to turn your code into an algorithm, I will
endeavor to look at it.

I don't do Python.

--
Tim Wescott
Control system and signal processing consulting
www.wescottdesign.com
```
```Nasser M. Abbasi wrote:

> On 6/8/2010 6:55 PM, HardySpicer wrote:
>
> > On Jun 9, 7:41 am, "grzegorz g."
> >
> > Why the Hell Python? Why not use Javascript while you're at it!
>
> fyi, Python is used a lot in mathematics software these days.

But its not used much for DSP code. For DSP its usually C, C++
or matlab/octave.

> Sage uses
> Python, Numpy is in Python, as well as SciPy, and many more.  Many
> people write scientific software in Python.

Many people drink too much alcohol and then drive motor vehicles.
That still doesn't make it a good idea.

Erik
--
----------------------------------------------------------------------
Erik de Castro Lopo
http://www.mega-nerd.com/
```
```On Jun 8, 8:44&#2013266080;pm, Tim Wescott <t...@seemywebsite.now> wrote:
> I don't do Python.
Python with numpy and scipy is a good alternative to Scilab.
At least there is a real debugger.

Peter Nachtwey
```
```On 6/8/2010 9:05 PM, Erik de Castro Lopo wrote:
> Nasser M. Abbasi wrote:
>
>> On 6/8/2010 6:55 PM, HardySpicer wrote:
>>
>>> On Jun 9, 7:41 am, "grzegorz g."
>>>
>>> Why the Hell Python? Why not use Javascript while you're at it!
>>
>> fyi, Python is used a lot in mathematics software these days.
>
> But its not used much for DSP code. For DSP its usually C, C++
> or matlab/octave.
>
>> Sage uses
>> Python, Numpy is in Python, as well as SciPy, and many more.  Many
>> people write scientific software in Python.

Or FORTRAN.

> Many people drink too much alcohol and then drive motor vehicles.
> That still doesn't make it a good idea.
>
> Erik

The cool kids still use Logo.

Or so I'm told...

--
Eric Jacobsen
Minister of Algorithms
Abineau Communications
http://www.abineau.com
```
```ok, I see that you (DSP guys) use a matlab/octave and it's more popular (in
this area). However:
1. nobody said why python is so shity in DSP
2. I don't really care - either you can help me or discuss about your
disapprovement for python somewhere else.

ctrl+c,ctrl+v was a bad idea, so I upload it:

http://www.2shared.com/file/f_G2QtG6/kalman.html

```
```Do you really expect help now?

---
frmsrcurl: http://compgroups.net/comp.dsp/kalman-filter-python-implementation
```