from mpmath import *
import filter


aoList = [[matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['2.0'],
 ['2.20548920717302'],
 ['1.02685549688771']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['2.0'],
 ['2.08776684562391'],
 ['0.0367503387694498']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['2.0'],
 ['1.30546553784744'],
 ['0.0354396553196523']])], [matrix(
[['0.0'],
 ['1.0'],
 ['0.0']]), matrix(
[['2.0'],
 ['1.82405607288538'],
 ['-0.76987498391962']])], [matrix(
[['0.0'],
 ['0.0'],
 ['3.14159265358979']]), matrix(
[['4.0'],
 ['3.71918454905722'],
 ['0.35916207728466']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['4.0'],
 ['2.83717093252977'],
 ['0.509587721653303']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['2.0'],
 ['2.41183680434247'],
 ['0.53917963642527']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['2.0'],
 ['1.66711158360596'],
 ['0.730797774257105']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['1.0'],
 ['3.65944512645249'],
 ['0.552953138159899']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['1.0'],
 ['3.00581078503927'],
 ['0.995072035841047']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['3.0'],
 ['4.50728688290551'],
 ['0.261805025687585']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['3.0'],
 ['3.60140886761014'],
 ['0.327424469090921']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['4.0'],
 ['3.45773283042284'],
 ['-0.3466393945184']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['4.0'],
 ['2.32291804386225'],
 ['-0.273044948290182']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['2.0'],
 ['2.31262603676486'],
 ['0.50151016284571']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['2.0'],
 ['1.69775480024422'],
 ['1.12880497304794']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['1.0'],
 ['3.9788483645699'],
 ['0.51451826623176']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['1.0'],
 ['3.2440890929547'],
 ['0.344587257456536']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['3.0'],
 ['4.25406703780948'],
 ['0.0938070227790705']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['3.0'],
 ['3.18983369547146'],
 ['-0.0598486595876433']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['4.0'],
 ['2.82621355340813'],
 ['-0.159641599679326']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['4.0'],
 ['1.98292343384783'],
 ['-0.431050161440965']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['2.0'],
 ['2.51873742796164'],
 ['0.0726714585794996']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['2.0'],
 ['1.45111653833545'],
 ['0.265576764059834']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['1.0'],
 ['3.78177672728935'],
 ['0.551264586737432']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['1.0'],
 ['3.20064348175051'],
 ['0.666554502934465']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['3.0'],
 ['4.59798059311442'],
 ['0.432195624557356']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['3.0'],
 ['3.91324060574617'],
 ['0.690209291192057']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['4.0'],
 ['3.56766555160478'],
 ['0.0283580034084693']])], [matrix(
[['1.0'],
 ['0.0'],
 ['0.0']]), matrix(
[['4.0'],
 ['2.46984461916474'],
 ['0.111901144142095']])], [matrix(
[['0.0'],
 ['0.0'],
 ['1.5707963267949']]), matrix(
[['2.0'],
 ['2.3482923463627'],
 ['0.735683511210234']])]]

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.T

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

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