Prev: Postdoc position in Image and Waveform Compression
Next: NEED HELP - MUSICAL INSTRUMENT INVENTOR NEEDS A BRILLIANT DSP DESIGNER ASAP!
From: grzegorz g. on 8 Jun 2010 15:41
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 ?









From: Michael_RW on 8 Jun 2010 21:39
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
From: HardySpicer on 8 Jun 2010 21:55
On Jun 9, 7:41 am, "grzegorz g."
<grzegorz.gwardys(a)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:  [[ -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 ?

Why the Hell Python? Why not use Javascript while you're at it!
From: Nasser M. Abbasi on 8 Jun 2010 23:18
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

From: Tim Wescott on 8 Jun 2010 23:44
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
 |  Next  |  Last
Pages: 1 2 3 4 5
Prev: Postdoc position in Image and Waveform Compression
Next: NEED HELP - MUSICAL INSTRUMENT INVENTOR NEEDS A BRILLIANT DSP DESIGNER ASAP!