kalman filter - python implementation

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