# 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)[0])#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][0]=teta_list[n]
#X[n][1]=omega_list[n]
#X[n][2]=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[1])
#print B.shape=(3,3)
U = zeros((n_iter,X.shape[1]))
Q = eye(X.shape[1])
R = eye(X.shape[1])
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[0]

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&#4294967295;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: &#4294967295;[[ -0.21369917]
> &#4294967295;[ &#4294967295;1.76860362]
> &#4294967295;[ &#4294967295;5.57973197]
> &#4294967295;[ 12.32486812]
> &#4294967295;[ 20.49270401]
> &#4294967295;[ 31.83940345]
> &#4294967295;[ 41.51642446]]
> X_estimate = &#4294967295;[ &#4294967295;0.00000000e+00 &#4294967295; 1.34490511e+00 &#4294967295; 4.33627110e+00 &#4294967295;
> 1.26596826e+01
> &#4294967295; &#4294967295;2.08888784e+01 &#4294967295;-3.25942333e+02 &#4294967295;-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):
> &#4294967295; &#4294967295; x = dot(A,x) + dot(B, u) &#4294967295;
> &#4294967295; &#4294967295; P = dot(A, dot(P, A.T)) + Q
> &#4294967295; &#4294967295; return(x,P)
>
> def update(x, P, z, H, R):
>
> &#4294967295; &#4294967295; Hx = dot(H, x)
> &#4294967295; &#4294967295; IS = R + dot(H, dot(P, H.T))
> &#4294967295; &#4294967295; PH = dot(P,H.T)
> &#4294967295; &#4294967295; K= dot(PH,inv(IS))
> &#4294967295; &#4294967295; x = x + dot(K, (z-Hx)[0])#by miec skalar, a nie wektor 1 elementowy ...
>
> &#4294967295; &#4294967295; P = P - dot(K, dot(H, P))
> &#4294967295; &#4294967295; return (x,P)
>
> def KalmanInit():
> &#4294967295; &#4294967295; # intial parameters
> &#4294967295; &#4294967295; n_iter = 7 #iteration number
> &#4294967295; &#4294967295; #X - state vector = [teta,omega, alfa]
> &#4294967295; &#4294967295; #teta - angle
> &#4294967295; &#4294967295; #omega(n)= [teta(n+1)-teta(n-1)]/2 &#4294967295;temporary freq.
> &#4294967295; &#4294967295; #alfa(n)=(teta(n+1)-teta(n))- (teta(n)-teta(n-1))=
> teta(n+1)-2teta(n)+teta(n-1) modulation factor
> &#4294967295; &#4294967295; X=zeros((n_iter,3))
> &#4294967295; &#4294967295; X_estimate=zeros((n_iter,3))
> &#4294967295; &#4294967295; X_predict=zeros((n_iter,3))
> &#4294967295; &#4294967295; teta_list=zeros(n_iter+1) &#4294967295;
> &#4294967295; &#4294967295; omega_list=zeros(n_iter) &#4294967295;
> &#4294967295; &#4294967295; alfa_list=zeros(n_iter) &#4294967295;
>
> &#4294967295; &#4294967295; for n in range(n_iter+1):
> &#4294967295; &#4294967295; &#4294967295; &#4294967295; teta_list[n]=n+n*n
>
> &#4294967295; &#4294967295; #temporary freq. and modulation factor - not important if H= [1 0 0] &#4294967295;
>
> &#4294967295; &#4294967295; #for n in range(1,n_iter):# dla n = 0 -> omega,alfa=0
> &#4294967295; &#4294967295; # &#4294967295; &#4294967295;omega_list[n]=(teta_list[n+1]-teta_list[n-1])/2
> &#4294967295; &#4294967295; # &#4294967295; &#4294967295;alfa_list[n]=teta_list[n+1]-2*teta_list[n]+teta_list[n-1]
> &#4294967295; &#4294967295; for n in range(n_iter):
> &#4294967295; &#4294967295; &#4294967295; &#4294967295; X[n][0]=teta_list[n]
> &#4294967295; &#4294967295; &#4294967295; &#4294967295; #X[n][1]=omega_list[n]
> &#4294967295; &#4294967295; &#4294967295; &#4294967295; #X[n][2]=alfa_list[n]
>
> &#4294967295; &#4294967295; X=array(X)
> &#4294967295; &#4294967295; A &#4294967295;= array([[1, 1, 0.5], [0, 1, 1], [0, 0, 1]])
> &#4294967295; &#4294967295; H = array([1, 0, 0])
> &#4294967295; &#4294967295; B = eye(X.shape[1])
> &#4294967295; &#4294967295; #print B.shape=(3,3)
> &#4294967295; &#4294967295; U = zeros((n_iter,X.shape[1]))
> &#4294967295; &#4294967295; Q = eye(X.shape[1])
> &#4294967295; &#4294967295; R = eye(X.shape[1])
> &#4294967295; &#4294967295; P_init &#4294967295;= diag((0.01, 0.01, 0.01)) &#4294967295; &#4294967295;
> &#4294967295; &#4294967295; Z=zeros((n_iter,1))
> &#4294967295; &#4294967295; V = normal(0,1,n_iter)
>
> &#4294967295; &#4294967295; for n in range(n_iter):
> &#4294967295; &#4294967295; &#4294967295; &#4294967295; Z[n]=dot(H,X[n])+V[n]
>
> &#4294967295; &#4294967295; return n_iter,X_estimate,X_predict,A,H,Q,B,U,R,P_init,Z
>
> def ccmKalman():
>
> &#4294967295; &#4294967295; n_iter,X_estimate,X_predict,A,H,Q,B,U,R,P_estimate,Z=KalmanInit()
> &#4294967295; &#4294967295; print "observation: ", Z
>
> &#4294967295; &#4294967295; for n in range(1,n_iter):
> &#4294967295; &#4294967295; &#4294967295; &#4294967295; (X_predict[n],P_predict)=predict(X_estimate[n-1],P_estimate, A, Q,
> B, U[n-1])
> &#4294967295; &#4294967295; &#4294967295; &#4294967295; (X_estimate[n],P_estimate)=update(X_predict[n],P_predict, Z[n], H,
> R)
>
> &#4294967295; &#4294967295; print "X_estimate = ", X_estimate.T[0]
>
> 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)[0])#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][0]=teta_list[n]
>          #X[n][1]=omega_list[n]
>          #X[n][2]=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[1])
>      #print B.shape=(3,3)
>      U = zeros((n_iter,X.shape[1]))
>      Q = eye(X.shape[1])
>      R = eye(X.shape[1])
>      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[0]
>
>
> 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&#4294967295;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.

```Do you really expect help now?