Nonlinear function in Extended Kalman Filter

1

hello,
I am using Pyros built-in classes from the Extended Kalman Filter 'EKFState" and “EKFDistribution”,
also used in the tutorial https://pyro.ai/examples/ekf.html to implement a version of the EKF with a nonlinear function h(x), that transforms the (x,y) coordinates of an object into measurements from 4 different cameras.

# The matrix C are the coordinates of the 4 cameras and xvalue are the real x,y coordinates of the object
def hpyro(xvalue):
    [x,y] = np.zeros(shape=(2,4))
    xvalue = xvalue.detach().numpy()
    y[[0,2]] = xvalue[1]-C[[0,2],1] # Camera 1 and 3
    x[[0,2]] = xvalue[0]-C[[0,2],0]
    y[[1,3]] = xvalue[0]-C[[1,3],0] # Camera 2 and 4
    x[[1,3]] = xvalue[1]-C[[1,3],1]
    
    p = d*(y/x) # Camera observations
    return x,y,p

I want to use this function as the function h(x) in the Extended Kalman Filter equations from the tutorial. I also noted that inside the class “EKFDistribution” the function “PositionMeasurement” is used. Looking at the source code of PositionMeasurement : pyro.contrib.tracking.measurements — Pyro documentation,
this one seems to use some measurement map h(). However, it uses its own function h(.) but I want to change that to my own function. I however don’t see where this PositionMeasurement obtains this h(.) function and how I can change it.

My question is:

How can I use my own nonlinear function h(x) inside the built in Extended Kalman Filter classes? More specifically how can I use it within the PositionMeasurement / Measurement functions?

For more information the Jacobian H for function h(x) would look like this:

        [xtemp,ytemp,hpred] = hpyro(x_pv)
        vpar = 4
        H = np.zeros(shape=(4,vpar))
        H[0,0:2] = np.array([-ytemp[0]/((xtemp[0])**2), 1/xtemp[0]]) # Camera 1
        H[2,0:2] = np.array([-ytemp[2]/((xtemp[2])**2), 1/xtemp[2]]) # Camera 3
        H[1,0:2] = np.array([1/xtemp[1],-ytemp[1]/((xtemp[1])**2)])  # Camera 2
        H[3,0:2] = np.array([1/xtemp[3],-ytemp[3]/((xtemp[3])**2)]) # Camera 4
        H =torch.from_numpy(H) 
        z_predicted = hpred

Thanks in advance if anyone can help :slight_smile: