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-C[[0,2],1] # Camera 1 and 3 x[[0,2]] = xvalue-C[[0,2],0] y[[1,3]] = xvalue-C[[1,3],0] # Camera 2 and 4 x[[1,3]] = xvalue-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 : http://docs.pyro.ai/en/1.5.1/_modules/pyro/contrib/tracking/measurements.html,
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/((xtemp)**2), 1/xtemp]) # Camera 1 H[2,0:2] = np.array([-ytemp/((xtemp)**2), 1/xtemp]) # Camera 3 H[1,0:2] = np.array([1/xtemp,-ytemp/((xtemp)**2)]) # Camera 2 H[3,0:2] = np.array([1/xtemp,-ytemp/((xtemp)**2)]) # Camera 4 H =torch.from_numpy(H) z_predicted = hpred
Thanks in advance if anyone can help