right we're attempting create 5 dimensional kalman filter recieves input x , y coordinates of small bug wandering around in box. observe bug makes 2000 movements, predict it's position forward. dimensions x-coord, y-coord, velocity, heading, , angular acceleration. following code have far.
#x list of 5 variables - x, y, velocity, angularvelocity, angularacceleration #currentmeasurement x , y observed def kalman_filter(x, p, currentmeasurement, lastmeasurement = none): prevmeasurement = [] #if there lastmeasurement argument, becomes measurement if lastmeasurement: prevmeasurement = [lastmeasurement[0], lastmeasurement[1], x.value[3][0]] #if there no lastmeasurement argument, current measurement becomes measurement. else: prevmeasurement = [x.value[0][0], x.value[1][0], x.value[3][0]] #prediction step = x.value[3][0] f = matrix([ [1., 0., cos(a), 0., 0.], [0., 1., sin(a), 0., 0.], [0., 0., 1., 0., 0.], [0., 0., 0., 1., dt], [0., 0., 0., 0., 1.]]) x = (f * x) + u p = f * p * f.transpose() #we can calculate heading our observations. heading = atan2(currentmeasurement[0][1] - prevmeasurement[1], currentmeasurement[0][0] - prevmeasurement[0]) while(abs(heading - prevmeasurement[2]) > pi): heading = heading + 2*pi*((prevmeasurement[2]-heading)/abs(prevmeasurement[2]-heading)) #perhaps velocity should calculated? # measurement update dt = 1. u = matrix([[0.], [0.], [0.], [0.], [0.]]) # external motion h = matrix([[1., 0., 0., 0., 0.], #the measurement function [0., 1., 0., 0., 0.], [0., 0., 0., 1., 0.]]) r = matrix([[1., 0., .0], #measurement uncertainty [0., 1., .0], [0., 0., 1.]]) = matrix([[]]) #a 5x5 identity matrix i.identity(5) prevmeasurement = [currentmeasurement[0][0], currentmeasurement[0][1], heading] z = matrix([prevmeasurement]) y = z.transpose() - (h * x) s = h * p * h.transpose() + r k = p * h.transpose() * s.inverse() x = x + (k * y) p = (i - (k * h)) * p return x,p this resulting in wildly incorrect estimations. we're not sure we're doing wrong here - think we're following of steps correctly, not sure if have of required matrices correct. input helpful!
the top comment has wrong description. state must x, y, velocity, angle, angularvelocity
you're missing q, process covariance. should reflect how state can change between updates, , added update of p in predict step.
you're building ekf (since update requires trig, nonlinear). you've constructed matrix f performs state update, need process covariance update jacobian of update function. in case looks like:
j = matrix([ [1., 0., cos(a), -sin(a), 0.], [0., 1., sin(a), cos(a), 0.], [0., 0., 1., 0., 0.], [0., 0., 0., 1., dt], [0., 0., 0., 0., 1.]]) if direct measurement position, should not compute heading , use measurement. let kf it.
you should set r real uncertainties of measurement. noise represented q, r, , propagated in p more important state x if want kf work.
