from mpmath import *
import filter

aoList=[[matrix([[0]]), matrix([[0]])],
		[matrix([[0]]), matrix([[1]])],
		[matrix([[1]]), matrix([[2]])],
		[matrix([[1]]), matrix([[4]])],
		[matrix([[-2]]), matrix([[7]])],
		[matrix([[3]]), matrix([[8]])],
		[matrix([[-4]]), matrix([[12]])],
		[matrix([[1]]), matrix([[12]])],
		[matrix([[0]]), matrix([[13]])],
		[matrix([[0]]), matrix([[14]])],
		[matrix([[0]]), matrix([[15]])]]

filter.setInitialStateUniform()

print filter.getMean()
print filter.getSigma()

step = 0

for ao in aoList:
	a = ao[0]
	o = ao[1]
	
	print "step:", step
	
	print "A:", a

	filter.motionUpdate(a)
	
	print "mu:",filter.getMean()
	print "Sig:",filter.getSigma()

	print "O:", o
	
	filter.observationUpdate(o)
	
	print "mu:",filter.getMean()
	print "Sig:",filter.getSigma()
	
	#print "Chol",cholesky(filter.getSigma())
	
	step = step + 1