Started translating PBVI code from matlab to Java