To illustrate what I mean by "expressing systems of equations in a declarative way", here is a toy-example of how to estimate the position of the a sensor with respect to a robot's center if you have access to a dataset containing robot positions and the sensor positions. The 'naive' approach (it very often works) is to solve an over-constrained system with a gradient-descent. To perform a gradient descent, you need a residual function and its jacobian. Here's how you would do to compute it with Sympy. (Note: you'd just have to define the `transform` and `invert` functions...)
# Pose of a sensor in robot frame (to be estimated)
xa, ya = symbols("xa, ya")
a = Matrix([xa, ya, ta])
# Position of the robot at time t
rx, ry, rt = symbols("rx, ry, rt")
rk = Matrix([rx, ry, rt])
# Measure of the sensor at time t
gx, gy = symbols("gx, gy")
gk = Matrix([gx, gy, 0])
# Estimated x (from the measures)
estimated_a = transform(invert(rk), gk)
# compute the norm of the gk, squared
n2_mat = norm2(estimated_a - a)
n2 = sympy.collect(sympy.expand(n2_mat[0, 0]), a).simplify()
# Compute the jacobian
J = n2_mat.jacobian([xa, ya, ta])
# Print what is necessary for Guass-Markov regression
print("\n\nres =", n2)
print("\nJacobian = ", J)