import numpy as np

import matplotlib.pyplot as plt

import seaborn

from mpl$\_$toolkits.mplot$3$d import Axes$3$D

class HMM$($object$)$:

def $\_\_$init$\_\_($self$,$ transition$\_$matrix$\_$matrix,current$\_$state$)$:

self.transition$\_$matrix $=$ transition$\_$matrix

self.current$\_$state $=$ current$\_$state

def filtering$($self$,$observation$\_$matrix$)$:

new$\_$state $=$ np$.$dot$($observation$\_$matrix,np$.$dot$($self$.$transition$\_$matrix,self.current$\_$state$))$

new$\_$state$\_$normalized $=$ new$\_$state$/$np$.$sum$($new$\_$state$)$

self.current$\_$state $=$ new$\_$state$\_$normalized

return new$\_$state$\_$normalized

def prediction$($self$)$:

new$\_$state $=$ np$.$dot$($self$.$transition$\_$matrix,self.current$\_$state$)$

new$\_$state$\_$normalized $=$ new$\_$state$/$np$.$sum$($new$\_$state$)$

self.current$\_$state$=$new$\_$state$\_$normalized

return new$\_$state$\_$normalized

def plot$\_$state$($self$)$:

fig $=$ plt$.$figure$()$

ax$1=$ fig.add$\_$subplot$(111,$ projection$=\text{'}3$d$\text{'})$

xpos $=[0,0,1,2,2,1]$

ypos $=[0,1,1,1,0,0]$

zpos $=$ np$.$zeros$($len$($initial$\_$state.shape$))$

dx $=$ np$.$ones$($len$($initial$\_$state.shape$))$

dy $=$ np$.$ones$($len$($initial$\_$state.shape$))$

dz $=$ self.current$\_$state

ax$1.$bar$3$d$($xpos$,$ ypos, zpos, dx$,$ dy$,$ dz$,$ color$=\text{'}$#ce$8900\text{'})$

ax$1.$set$\_$xticks$([0.,1.,2.,3.])$

ax$1.$set$\_$yticks$([0.,1.,2.])$

plt$.$show$()$

def create$\_$observation$\_$matrix$($self$,$error$\_$rate, no$\_$discrepancies$)$:

sensor$\_$list$=[]$

for number in no$\_$discrepancies:

probability$=(1-$error$\_$rate$)**(4-$number$)*$error$\_$rate$**$number

sensor$\_$list.append$($probability$)$

observation$\_$matrix $=$ np$.$zeros$(($len$($sensor$\_$list$),$len$($sensor$\_$list$)))$

np$.$fill$\_$diagonal$($observation$\_$matrix,sensor$\_$list$)$

return observation$\_$matrix

# step $1$: define initial$\_$state

# initial$\_$state

# step $2$: compute transition matrix

# transition$\_$matrix

# step $3$: based on the given observations from sensor readings, compute the observation matrix, the error rate is $0\mathrm{.}25$

# sensor reading: $[$SWE$,$ NW$,$ N$,$ NE$]$

Model $=$ HMM$($transition$\_$matrix,initial$\_$state$)$

Model$2=$ HMM$($transition$\_$matrix,initial$\_$state$)$

# localize of the robot using filtering

state$\_1=$ Model.filtering$($observation$\_$matrix$\_$SWE$)$

Model.plot$\_$state$()$

state$\_2=$ Model.filtering$($observation$\_$matrix$\_$NW$)$

Model.plot$\_$state$()$

state$\_3=$ Model.filtering$($observation$\_$matrix$\_$N$)$

Model.plot$\_$state$()$

state$\_4=$ Model.filtering$($observation$\_$matrix$\_$NE$)$

Model.plot$\_$state$()$

state$\_5=$ Model.filtering$($observation$\_$matrix$\_$SWE$)$

Model.plot$\_$state$()$

# localize of the robot using filtering $($three first timesteps$)$ and prediction $($two last timesteps$)$

state$\_6=$ Model$2.$filtering$($observation$\_$matrix$\_$SWE$)$

Model$2.$plot$\_$state$()$

state$\_7=$ Model$2.$filtering$($observation$\_$matrix$\_$NW$)$

Model$2.$plot$\_$state$()$

state$\_8=$ Model$2.$filtering$($observation$\_$matrix$\_$N$)$

Model$2.$plot$\_$state$()$

prediction$\_1=$ Model$2.$prediction$()$

Model$2.$plot$\_$state$()$

prediction$\_2=$ Model$2.$prediction$()$

Model$2.$plot$\_$state$()$