from jhplot  import *
from java.util import Random
from jhplot.math.kalman import JKalman 
from jhplot.math.kalman.jama import Matrix

kal=JKalman(4,2)
rand =Random()

x,y=0,0
dx,dy=rand.nextDouble(),rand.nextDouble()
s =Matrix(4, 1)     #  state [x, y, dx, dy, dxy]        
c = Matrix(4, 1)    # corrected state [x, y, dx, dy, dxy]                

m = Matrix(2, 1)   #  measurement [x]
m.set(0, 0, x)
m.set(1, 0, y)

# transitions for x, y, dx, dy
tr = [ [1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1] ]
kal.setTransition_matrix(Matrix(tr))

# 1s somewhere?
kal.setError_cov_post(kal.getError_cov_post().identity())
print "first x:" + str(x) + ", y:" +str(y) + ", dx:" + str(dx) + ", dy:" + str(dy)

print "no; x; y; dx; dy; predictionX; predictionY; predictionDx; predictionDy; correctionX; correctionY; correctionDx; correctionDy;";
for i in range(200):
     s = kal.Predict()                 # check state before
     m.set(1, 0, rand.nextDouble())  # function init
     x = rand.nextGaussian()
     y = rand.nextGaussian()
     m.set(0, 0, m.get(0, 0) + dx + rand.nextGaussian());
     m.set(1, 0, m.get(1, 0) + dy + rand.nextGaussian());
     # a missing value (more then 1/4 times)
     if rand.nextGaussian() < -0.8:
             print "", i , ";;;;;", s.get(0, 0), ";", s.get(1, 0), ";" , s.get(2, 0) , ";" , s.get(3, 0), ";"
else:   #  measurement is ok : 
      c = kal.Correct(m) # look better
      print "" , i, ";" , m.get(0, 0), ";" , m.get(1, 0), ";", x, ";", y , ";", s.get(0, 0) ,";",s.get(1, 0), ";" ,s.get(2, 0) ,";",s.get(3, 0), ";",c.get(0, 0), ";", c.get(1, 0), ";", c.get(2, 0), ";" ,c.get(3, 0),";"