Homogenous Transformation for Particle Filter

homogenous_transformation.py

import numpy as np
import math

def h_transform(observation_coords, particle_coords, particle_heading):
    mat = np.matrix([
        [math.cos(particle_heading), -math.sin(particle_heading), particle_coords[0]],
        [math.sin(particle_heading), math.cos(particle_heading),  particle_coords[1]],
        [0,                          0,                           1]
    ])
    vec = np.matrix([
        [observation_coords[0]],
        [observation_coords[1]],
        [1]
    ])
    ret = mat * vec
    return (ret.item(0), ret.item(1))

observation_coords = (0,-4)
particle_coords = (4,5)
particle_heading = -math.pi / 2
print(h_transform(observation_coords, particle_coords, particle_heading))