Closed a1650078 closed 3 years ago
Hi,
actually the ExtendedKalmanFilter class support nonlinear functions for both state transition model f()
and measurement model h()
.
A fully worked example of how to implement these models can be found in the EKF chaper in rlabbs book (see section "Robot Motion Model")
The move(self, x, u, dt)
function corresponds to your state transition functionf()
. Additionally to that you have to define your own predict()
function.
Example for a nonlinear state transition function:
hdg = x[2, 0]
vel = u[0]
steering_angle = u[1]
dist = vel * dt
if abs(steering_angle) > 0.001: # is robot turning?
beta = (dist / self.wheelbase) * tan(steering_angle)
r = self.wheelbase / tan(steering_angle) # radius
dx = np.array([[-r*sin(hdg) + r*sin(hdg + beta)],
[r*cos(hdg) - r*cos(hdg + beta)],
[beta]])
else: # moving in straight line
dx = np.array([[dist*cos(hdg)],
[dist*sin(hdg)],
[0]])
return x + dx
Thanks PGotzmann. I'm sure the examples will be helpful for someone looking for an out-of-the-box EKF implementation in the future. When I opened this issue, I realised the code could be changed / overriden to get the same effect, but from the users perspective as someone that already knows about EKF's / numpy / smpy I might as well do it myself at that point. I was hoping I could use the library as is to avoid having to debug one myself. An EKF which takes F as a matrix by default is unclear in my opinion as a function F is the assumption (like a quadratic solver for ax + b = 0).
I might be misreading the documentation but it seems that the ExtendedKalmanFilter class does not actually implement a full EKF. I was expecting to use models like
but it looks like only the measurement can be a general function.