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 ?
kalman filter - python implementation
Started by ●June 8, 2010
Reply by ●June 8, 20102010-06-08
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
Reply by ●June 8, 20102010-06-08
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: �[[ -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!
Reply by ●June 9, 20102010-06-09
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
Reply by ●June 9, 20102010-06-09
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
Reply by ●June 9, 20102010-06-09
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/
Reply by ●June 9, 20102010-06-09
On Jun 8, 8:44�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
Reply by ●June 9, 20102010-06-09
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. > > ErikThe cool kids still use Logo. Or so I'm told... -- Eric Jacobsen Minister of Algorithms Abineau Communications http://www.abineau.com
Reply by ●June 9, 20102010-06-09
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
Reply by ●June 9, 20102010-06-09
Do you really expect help now? --- frmsrcurl: http://compgroups.net/comp.dsp/kalman-filter-python-implementation
