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.)
