def EKF_update_dip(jaco, meas, x_pred, P_pred, R): c = jaco(x_pred[0], x_pred[1], x_pred[2]) g = np.dot(np.dot(P_pred, c.T), np.linalg.inv(np.dot(np.dot(c, P_pred), c.T) + R)) x_now = x_pred + np.dot(g, (meas - modD.calcB(x_pred, np.array([1.0, 0.0, 0])))) # dif = meas - calcB_dip(x_pred(1:3)',[1 0 0])' P_now = np.dot((np.eye(3) - np.dot(g, c)), P_pred) return (x_now, P_now)
H = np.array([1.,0.,0.]) Br = 12.6e+03 mu_0 = 4*np.pi*1e-07 mu_r = 1.05 addFact = 1 lamb = (Br*mu_0*mu_r)/(4*np.pi)*addFact factor = np.array([lamb, -lamb, lamb]) f = SXFunction([R], [factor*(((3*R*inner_prod(H,R))/(np.linalg.norm(R)**5)) - (H/(np.linalg.norm(R)**3)))]) f.init() r_test = DMatrix([0.04,0.,0.]) rOld = np.array([0.04,0.,0.]) f.setInput(r_test,0) f.evaluate() print "result casadi: ", f.getOutput(0) print "result model: ", modD.calcB(rOld,H) f_J = f.jacobian() # f_J *= 2 f_J.init() f_J.setInput(r_test,0) f_J.evaluate() print "result f_J: ", f_J.getOutput(0)
""" data simulation """ p = 0.5 h = np.array([1.0, 0.0, 0.0]) t = np.arange(0.04, 0.1, 0.001) r = np.zeros((len(t), 3)) b_sim = np.zeros((len(t), 3)) b_sim_noise = np.zeros((len(t), 3)) cnt = 0 for i in t: r[cnt] = np.array([i, 0.0, 0.0]) b_sim[cnt] = modD.calcB(r[cnt], h) b_sim_noise[cnt] = modD.calcB(r[cnt], h) + p * np.random.randn(1, 3) + [0.5, -1.8, 2.0] # b_sim_noise[cnt] = modD.calcB(r[cnt], h) + [0.5, -1.8, 2.0] cnt += 1 # plt.close('all') # plo.plotter2d((b_sim, b_sim_noise), ("simulated", "sim Noisy")) """ Kalman stuff """ # error covariance matrix (3x3), gets updated each step, so the initial one is not so important... P = np.eye(3) # process noise covariance matrix (3x3)
cnt += 1 #plo.plotter2d((p_dip,),("p",)) b_dip0 = np.zeros((len(r),3)) b_dip1 = np.zeros((len(r),3)) b_dip2 = np.zeros((len(r),3)) b_dip3 = np.zeros((len(r),3)) b_cyl0 = np.zeros((len(r),3)) b_cyl1 = np.zeros((len(r),3)) b_cyl2 = np.zeros((len(r),3)) b_cyl3 = np.zeros((len(r),3)) tmp = np.zeros((len(r),3)) cnt = 0 for i in p_dip: b_dip0[cnt] = modD.calcB(i-(s0+zOff), h0) b_dip1[cnt] = modD.calcB(i-(s1+zOff), h0) b_dip2[cnt] = modD.calcB(i-(s2+zOff), h0) b_dip3[cnt] = modD.calcB(i-(s3+zOff), h0) b_cyl0[cnt] = modC.calcB_cyl(i-(s0+zOff), 0.) b_cyl1[cnt] = modC.calcB_cyl(i-(s1+zOff), 0.) b_cyl2[cnt] = modC.calcB_cyl(i-(s2+zOff), 0.) b_cyl3[cnt] = modC.calcB_cyl(i-(s3+zOff), 0.) tmp[cnt] = (i-(s0+zOff)) # b_dip0[cnt] = modD.calcB(i, h0) # b_cyl0[cnt] = modC.calcB_cyl(i, 0.)