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 ?

0 |

6/8/2010 7:41:34 PM

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

1 |

6/9/2010 3:18:16 AM

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

-1 |

6/9/2010 3:44:38 AM

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/

-1 |

6/9/2010 4:05:16 AM

On Jun 8, 8:44=A0pm, 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

0 |

6/9/2010 5:13:54 AM

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

-1 |

6/9/2010 5:29:23 AM

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

0 |

6/9/2010 9:14:33 AM

Do you really expect help now?

0 |

6/9/2010 1:20:02 PM

On 6/9/2010 9:20 AM, Michael wrote: > Do you really expect help now? > > --- > frmsrcurl: http://compgroups.net/comp.dsp/kalman-filter-python-implementation Why should he not? There was a time in this country when men wearing beards were pelted by kids in the street. I don't read Python, but from all I hear, it's suitable for this kind of work. Why the verbal brickbats? I note that they're not from anyone I've learned to respect. Jerry -- Engineering is the art of making what you want from things you can get. �����������������������������������������������������������������������

0 |

6/9/2010 1:42:22 PM

On 06/08/2010 10:13 PM, pnachtwey wrote: > 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. I was thinking as I wrote that that maybe I _should_ do Python. But for the moment I don't. -- Tim Wescott Control system and signal processing consulting www.wescottdesign.com

0 |

6/9/2010 2:22:07 PM

Jerry, I did not insult the poster or the fact the he is using Python as his programming language of choice - "brickbats" is harsh to say the least. I am, however, keenly aware of the tone of his last message. It is okay to be frustrated with a problem you are trying to solve, but extending it into a public forum where you ask individuals for their time and their experiences to help is another. Michael. -- Finding Nemo, Sharkbait - "He's probably American!"

0 |

6/9/2010 3:24:28 PM

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. > > Many people drink too much alcohol and then drive motor vehicles. > That still doesn't make it a good idea. > > Erik There's no reason not to. It's handy in that it really allows you to bang out algorithms quickly rather than getting caught up in implementational details. Then if the ultimate target is some overpowered Core Duo, you might as well leave it Python (the speed hit's not as bad as you think), or if it's not then you rewrite it in C, C++, Ada, Forth, assembly, VHDL, etc depending on your target, but at least you know that if it doesn't work the error's in the porting, not the algorithm. -- Rob Gaddi, Highland Technology Email address is currently out of order

0 |

6/9/2010 4:35:31 PM

>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 > Write the exact equations you used for each step, and number the steps. Don't algebraicly change them in any way (i.e., write the equations exactly as implemented). State the precision used. Describe the problem (inputs/outputs/model) of the filter. And if you decide to upload stuff, maybe consider putting it someplace not littered with popups, etc. It doesn't sound like many people wish to look at your code; this is reasonable regardless of the language. If *you* know Python sufficiently well, you should have no trouble taking what you've learned here back into your language of choice.

0 |

6/9/2010 4:43:53 PM

On 06/09/2010 09:35 AM, Rob Gaddi wrote: > 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. >> >> Many people drink too much alcohol and then drive motor vehicles. >> That still doesn't make it a good idea. >> >> Erik > > There's no reason not to. It's handy in that it really allows you to > bang out algorithms quickly rather than getting caught up in > implementational details. Then if the ultimate target is some > overpowered Core Duo, you might as well leave it Python (the speed hit's > not as bad as you think), or if it's not then you rewrite it in C, C++, > Ada, Forth, assembly, VHDL, etc depending on your target, but at least > you know that if it doesn't work the error's in the porting, not the > algorithm. > Which is exactly what you'd say about Scilab, Octave, Matlab, etc. -- Tim Wescott Control system and signal processing consulting www.wescottdesign.com

0 |

6/9/2010 4:48:59 PM

[cut] > Can someone help me, cos I have no idea what's wrong ? Mmm... a bit too much code there :) Why not you try to the same computation using... err... octave?... and check numbers step-by-step to understand where the error is? I do python, so I could help... do you want to share directly .py files?

0 |

6/9/2010 5:26:12 PM

On Jun 9, 3:18=A0pm, "Nasser M. Abbasi" <n...@12000.org> 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. Sage uses > Python, Numpy is in Python, as well as SciPy, and many more. =A0Many > people write scientific software in Python. > > --Nasser What the hell is Sage and Numpty? Ever heard of Matlab? Far easier.

0 |

6/9/2010 7:07:36 PM

>Jerry, > >I did not insult the poster or the fact the he is using Python as his programming language of choice - "brickbats" is harsh to say the least. I am, however, keenly aware of the tone of his last message. > >It is okay to be frustrated with a problem you are trying to solve, but extending it into a public forum where you ask individuals for their time and their experiences to help is another. > > >Michael. Michael, I also didn't insult anybody and I didn't extend a frustration. I only reacted on offtop (often agressive, without any arguments) >[cut] >> Can someone help me, cos I have no idea what's wrong ? > > >Mmm... a bit too much code there :) >Why not you try to the same computation using... err... octave?... and >check numbers step-by-step to understand where the error is? > > >I do python, so I could help... do you want to share directly .py >files? > I apreciate it very much, py file is here (no popups): http://home.elka.pw.edu.pl/~ggwardys/ thx ! :)

0 |

6/9/2010 7:12:26 PM

On 06/09/2010 12:07 PM, HardySpicer wrote: > On Jun 9, 3:18 pm, "Nasser M. Abbasi"<n...@12000.org> 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. Sage uses >> Python, Numpy is in Python, as well as SciPy, and many more. Many >> people write scientific software in Python. >> >> --Nasser > > What the hell is Sage and Numpty? Ever heard of Matlab? Far easier. Until you go to distribute it, and find out that you have to pay enough to buy a really nice car for each copy of Matlab that you give away. Matlab is _expensive_. While it delivers considerable value, there are free alternatives that do as well or better than Matlab. From a standpoint of how much you gain vs. how much you pay, Matlab is _horrible_. -- Tim Wescott Control system and signal processing consulting www.wescottdesign.com

0 |

6/9/2010 7:13:50 PM

Tim Wescott wrote: > Matlab is _expensive_. Octave is a cheap alternative. > While it delivers considerable value, there are > free alternatives that do as well or better than Matlab. From a > standpoint of how much you gain vs. how much you pay, Matlab is _horrible_. I agree. For prototyping in a high level language I usually use Ocaml or Haskell, but I would never expect anyone not familiar with those languages to look at my code. Erik -- ---------------------------------------------------------------------- Erik de Castro Lopo http://www.mega-nerd.com/

0 |

6/9/2010 10:34:16 PM

On 06/09/2010 03:34 PM, Erik de Castro Lopo wrote: > Tim Wescott wrote: > >> Matlab is _expensive_. > > Octave is a cheap alternative. > >> While it delivers considerable value, there are >> free alternatives that do as well or better than Matlab. From a >> standpoint of how much you gain vs. how much you pay, Matlab is _horrible_. > > I agree. > I started using Scilab when the Octave project was in a shambles. Now the Scilab project is behind (still no debugger...) but I'm used to it. -- Tim Wescott Control system and signal processing consulting www.wescottdesign.com

0 |

6/9/2010 11:11:08 PM

grzegorz (Original Poster), With respect to your Python code, I suggest you have a look at the line, "IS = R + dot(H, dot(P, H.T))", in your "update" function definition. This goes back to a previous idea of this thread. As suggested by Michael (Plante), taking time to "Describe the problem (inputs/outputs/model) of the filter..." would help us understand the framework of your problem. Your code fails, I believe, because the "IS" matrix becomes ill-conditioned as the observations are processed. From what I can tell it stops at the 5th iteration. I think the root-problem lies in the respective sizes of the principle process/model vectors and matrices you are using. The "dot(H, dot(P, H.T))" portion of the above equation determines the innovation or residual covariance update. Looking at the sizes of the measurement/observation model vector (H) and the predicted (a priori) covariance estimate (P), might indicate the potential underlying issue. "H", as you've defined in your code, is 1-by-3 and "P" is 3-by-3. With reference to, "http://tinyurl.com/Wiki-Kalman-Reference", the size of the innovation covariance update is then: 1-by-3 times 3-by-3 times 3-by-1 yielding a 1-by-1 or scalar result. Should it not be a 3-by-3 matrix result in light of the 3-by-3 measurement covariance matrix, "R"? I am not 100% sure how Python handles the dimensions of the "array" and "mat" (matrix) types. In the event the measurement model vector's dimensions are reversed, then you have: 3-by-1 times 3-by-3 times 1-by-3 yielding a no-go invalid matrix result. With the scalar result, the same value is added to all elements of the measurement covariance matrix, "R". With continued processing, your innovation covariance grows resulting in an ill-conditioned matrix "IS" that can not be inverted. Perhaps you can flesh-out the vectors and matrices involved with your process and measurement models as suggested by Michael (Plante). From what I can see, you have: ---------- State Transition Matrix (A): [[ 1. 1. 0.5] [ 0. 1. 1. ] [ 0. 0. 1. ]] Process Noise Covariance Matrix (Q): [[ 1.0 0. 0. ] [ 0. 1.0 0. ] [ 0. 0. 1.0]] Measurement/Observation Model (H): [1 0 0] Measurement Noise Covariance Model (R): [[ 1.0 0. 0. ] [ 0. 1.0 0. ] [ 0. 0. 1.0]] Initial Covariance Estimate: [[ 0.01 0. 0.] [ 0. 0.01 0.] [ 0. 0. 0.01]] Initial State Estimate: [0 0 0] ---------- Application details would be helpful... There are sections of your code that you may also what to clarify. For example: # 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] This is used to generate the vector "X" later on in your code to be used as your measurement or observations vector. I hope all or part of this helps... Michael.

0 |

6/10/2010 7:15:46 AM

thx for help. the problem was a state matrix (A). after transposition it's ok (stupid error however difficult to find ... first good predictions made me think that matrices were ok)

0 |

6/29/2010 9:51:43 PM