def state_transition_function_jacobians():
# -------------- state transition function -----------------
# This is the nonlinear function f_r(X_r, U, N) that calculates the new robot pose.
# f_r forms part of the overall state transition function for all states, robot pose and landmarks, although the
# landmarks do not move and so there is no need to calculate partial derivatives for those states.
# The robot pose is defined as X_r = [x_r; y_r; alpha_r].
# The control input is defined as U = [x_u; alpha_u].
# The perturbation to the input is defined as N = [x_n; alpha_n].
x_r, y_r, alpha_r, x_u, alpha_u, x_n, alpha_n = sympy.symbols(
'x_r y_r alpha_r x_u alpha_u x_n alpha_n')
X_r = Matrix([[x_r], [y_r], [alpha_r]])
U = Matrix([[x_u], [alpha_u]])
N = Matrix([[x_n], [alpha_n]])
alpha_new = alpha_r + alpha_u + alpha_n
R = Matrix([[scos(alpha_new), -ssin(alpha_new)],
[ssin(alpha_new), scos(alpha_new)]])
X_r_new = Matrix([[x_r], [y_r]]) + R * Matrix([[x_u + x_n], [0]])
f_r = Matrix([[X_r_new[0]], [X_r_new[1]], [alpha_new]])
# ----------- Jacobian of f_r w.r.t X_r (the robot pose) --------------
d_f_r_by_X_r = f_r.jacobian(X_r)
# ------------ Jacobian of f_r w.r.t N (the noise perturbation) --------------
d_f_r_by_N = f_r.jacobian(N)
def observe_range_bearing_jacobians():
# --------------- nonlinear range-bearing sensor measurement function ----------------
# This function maps the landmark positions (in world rectangular coordinates) to range-bearing (local polar)
# coordinates.
# we need to use the estimated robot states in the Jacobian since the true states are not available to the estimator
x_r, y_r, alpha_r, l_i_x, l_i_y = sympy.symbols(
'x_r y_r alpha_r l_i_x l_i_y')
# transform landmark from world (rectangular) to local (rectangular) coordinates
R = Matrix([[scos(alpha_r), -ssin(alpha_r)],
[ssin(alpha_r), scos(alpha_r)]])
t = Matrix([x_r, y_r])
p_world = Matrix([l_i_x, l_i_y])
p_local_x, p_local_y = R.T * (p_world - t)
y_range = sqrt(p_local_x**2 + p_local_y**2)
y_bearing = atan2(p_local_y, p_local_x)
# calculate Jacobian in one go -> number of terms explodes!
h = Matrix([y_range, y_bearing])
# ---------- Jacobian of h w.r.t X_r (robot position) ---------
X_r = Matrix([[x_r], [y_r], [alpha_r]])
# calculate Jacobian in one go
d_h_r_by_X_r = h.jacobian(X_r)
# # calculate Jacobian in steps -> much fewer terms
#
# h1 = Matrix([p_local_x, p_local_y])
#
# p_local = Matrix([p_local_x, p_local_y])
# d_h1_by_p_local = h1.jacobian(p_local) # calculate first part of Jacobian
#
# # p_local_x_sym, p_local_y_sym = sympy.symbols('p_local_x_sym p_local_y_sym')
#
# # h2 = Matrix([y_range, y_bearing])
# # p_local_syms = Matrix([p_local_x_sym, p_local_y_sym])
# # d_h2_by_X_r = h2.jacobian(X_r) # calculate second part of Jacobian
# # # therefore d_h_by_X_r == d_h1_by_p_local * d_h2_by_X_r
#
# # ---------- Jacobian of h w.r.t L_i (landmark position) ---------
#
# L_i = Matrix([[l_i_x], [l_i_y]])
# d_h_r_by_L_i = h.jacobian(L_i)
# ---------- Jacobian of h w.r.t L_i (landmark position) ---------
L_i = Matrix([[l_i_x], [l_i_y]])
# calculate Jacobian in one go
d_h_r_by_L_i = h.jacobian(L_i)
print("test")
def _get_coordonnees(self):
M = self.__point
arc = self.__arc
O = arc.centre
a, b = arc._intervalle()
c = angle_vectoriel((1, 0), vect(O, M))
while c < a:
c += 2 * pi
# La mesure d'angle c est donc dans l'intervalle [a; a+2*pi[
if c > b: # c n'appartient pas à [a;b] (donc M est en dehors de l'arc de cercle)
if c - b > 2 * pi + a - c: # c est plus proche de a+2*pi
c = a
else: # c est plus proche de b
c = b
if b - a < contexte["tolerance"]:
return arc.point1.coordonnees
k = (b - c) / (b - a)
t = a * k + b * (1 - k)
x0, y0 = arc.centre.coordonnees
r = arc.rayon
if contexte["exact"] and issympy(r, x0, y0):
return x0 + r * scos(t), y0 + r * ssin(t)
else:
return x0 + r * cos(t), y0 + r * sin(t)
def _get_coordonnees(self):
M = self.__point
arc = self.__arc
O = arc.centre
a, b = arc._intervalle()
c = angle_vectoriel((1, 0), vect(O, M))
while c < a:
c += 2 * pi
# La mesure d'angle c est donc dans l'intervalle [a; a+2*pi[
if c > b: # c n'appartient pas à [a;b] (donc M est en dehors de l'arc de cercle)
if c - b > 2 * pi + a - c: # c est plus proche de a+2*pi
c = a
else: # c est plus proche de b
c = b
if b - a < contexte['tolerance']:
return arc.point1.coordonnees
k = (b - c) / (b - a)
t = a * k + b * (1 - k)
x0, y0 = arc.centre.coordonnees
r = arc.rayon
if contexte['exact'] and issympy(r, x0, y0):
return x0 + r * scos(t), y0 + r * ssin(t)
else:
return x0 + r * cos(t), y0 + r * sin(t)
def _get_coordonnees(self):
k = angle_vectoriel((1, 0), vect(self.__cercle.centre, self.__point))
x0, y0 = self.__cercle.centre.coordonnees
r = self.__cercle.rayon
if contexte["exact"] and issympy(r, x0, y0):
return x0 + r * scos(k), y0 + r * ssin(k)
else:
return x0 + r * cos(k), y0 + r * sin(k)
def _get_coordonnees(self):
k = angle_vectoriel((1, 0), vect(self.__cercle.centre, self.__point))
x0, y0 = self.__cercle.centre.coordonnees
r = self.__cercle.rayon
if contexte['exact'] and issympy(r, x0, y0):
return x0 + r * scos(k), y0 + r * ssin(k)
else:
return x0 + r * cos(k), y0 + r * sin(k)
def _get_coordonnees(self):
cercle = self.__cercle
k = self.__k
x0, y0 = cercle.centre.coordonnees
r = cercle.rayon
if contexte["exact"] and issympy(r, x0, y0):
return x0 + r * scos(k), y0 + r * ssin(k)
else:
return x0 + r * cos(k), y0 + r * sin(k)
def _get_coordonnees(self):
x0, y0 = self.__rotation.centre.coordonnees
xA, yA = self.__point.coordonnees
a = self.__rotation.radian
if contexte['exact'] and issympy(a, x0, y0, xA, yA):
sina = ssin(a) ; cosa = scos(a)
else:
sina = sin(a) ; cosa = cos(a)
return (-sina*(yA - y0) + x0 + cosa*(xA - x0), y0 + cosa*(yA - y0) + sina*(xA - x0))
def _get_coordonnees(self):
cercle = self.__cercle
k = self.__k
x0, y0 = cercle.centre.coordonnees
r = cercle.rayon
if contexte['exact'] and issympy(r, x0, y0):
return x0 + r * scos(k), y0 + r * ssin(k)
else:
return x0 + r * cos(k), y0 + r * sin(k)
def _get_coordonnees(self):
x0, y0 = self.__rotation.centre.coordonnees
xA, yA = self.__point.coordonnees
a = self.__rotation.radian
if contexte['exact'] and issympy(a, x0, y0, xA, yA):
sina = ssin(a)
cosa = scos(a)
else:
sina = sin(a)
cosa = cos(a)
return (-sina * (yA - y0) + x0 + cosa * (xA - x0),
y0 + cosa * (yA - y0) + sina * (xA - x0))
def inv_observe_range_bearing_jacobians():
# --------------- nonlinear inverse range-bearing measurement function ----------------
# This function maps the landmark positions (in world rectangular coordinates) to range-bearing (local polar)
# coordinates.
# we need to use the estimated robot states in the Jacobian since the true states are not available to the estimator
x_r, y_r, alpha_r, rho, psi = sympy.symbols('x_r y_r alpha_r rho psi')
# transform from polar to rectangular coordinates
x = rho * scos(psi)
y = rho * ssin(psi)
# transform from local rectangular coordinates to world rectangular coordinates
R = Matrix([[scos(alpha_r), -ssin(alpha_r)],
[ssin(alpha_r), scos(alpha_r)]])
t = Matrix([x_r, y_r])
p_local = Matrix([x, y])
g = p_world = R * p_local + t
# calculate Jacobian in one go -> number of terms explodes!
# ---------- Jacobian of g w.r.t X_r (robot position) ---------
X_r = Matrix([[x_r], [y_r], [alpha_r]])
# calculate Jacobian in one go
d_g_r_by_X_r = g.jacobian(X_r)
# ---------- Jacobian of g w.r.t y_i (range-bearing sensor measurement) ---------
y_i = Matrix([[rho], [psi]])
# calculate Jacobian in one go
d_h_r_by_y_i = g.jacobian(y_i)
print("test2")
def _get_coordonnees(self):
arc = self.__arc
a, b = arc._intervalle()
k = self.__k
if arc._sens() == 1:
t = a * k + b * (1 - k)
else:
t = b * k + a * (1 - k)
x0, y0 = arc.centre.coordonnees
r = arc.rayon
if contexte["exact"] and issympy(r, x0, y0):
return x0 + r * scos(t), y0 + r * ssin(t)
else:
return x0 + r * cos(t), y0 + r * sin(t)
def _get_coordonnees(self):
arc = self.__arc
a, b = arc._intervalle()
k = self.__k
if arc._sens() == 1:
t = a * k + b * (1 - k)
else:
t = b * k + a * (1 - k)
x0, y0 = arc.centre.coordonnees
r = arc.rayon
if contexte['exact'] and issympy(r, x0, y0):
return x0 + r * scos(t), y0 + r * ssin(t)
else:
return x0 + r * cos(t), y0 + r * sin(t)