from numpy import *

tam_mat=5
peso_mov=1
peso_obs=1

omega=matrix(zeros([tam_mat, tam_mat]))
sigma=matrix(zeros([tam_mat, tam_mat]))
xi=matrix(zeros(tam_mat)).T
mi=matrix(zeros(tam_mat)).T

def init(pos):
    omega[0,0]=1*peso_mov
    xi[0]=pos*peso_mov

def move(ind_orig, ind_dest, dist):
    omega[ind_orig, ind_orig]=omega[ind_orig, ind_orig]+(1*peso_mov)
    omega[ind_orig, ind_dest]=omega[ind_orig, ind_dest]-(1*peso_mov)
    xi[ind_orig]=xi[ind_orig]-(dist*peso_mov)
    omega[ind_dest, ind_dest]=omega[ind_dest, ind_dest]+(1*peso_mov)
    omega[ind_dest, ind_orig]=omega[ind_dest, ind_orig]-(1*peso_mov)
    xi[ind_dest]=xi[ind_dest]+(dist*peso_mov)
    
def observ(ind_orig, ind_dest, dist):
    omega[ind_orig, ind_orig]=omega[ind_orig, ind_orig]+(1*peso_obs)
    omega[ind_orig, ind_dest]=omega[ind_orig, ind_dest]-(1*peso_obs)
    xi[ind_orig]=xi[ind_orig]-(dist*peso_obs)
    omega[ind_dest, ind_dest]=omega[ind_dest, ind_dest]+(1*peso_obs)
    omega[ind_dest, ind_orig]=omega[ind_dest, ind_orig]-(1*peso_obs)
    xi[ind_dest]=xi[ind_dest]+(dist*peso_obs)

def resolv():
    sigma=omega.I
    mi=sigma*xi
    return [mi, sigma]

init(2)
move(0,1,5)
observ(1,4,9)
move(1,2,-1)
move(2,3,4)
observ(3,4,4)
[mi, sigma]=resolv()
print mi.T
print sigma