print("\t dP(landmark):")
        print(dP(S(0,0), landmark))
        print("\t P(observed_landmark): ", P(S(0,0), observed_landmark))

        print("\t dP*dS:")
        print(dP(S(0,0), landmark)*dS(0,0))

        print("\t dP*dS (fvp):")
        fvp = FeatureVisualPoint('fvp'+str(i))
        fvp.xy.value = (P(S(0,0), landmark)[0], P(S(0,0), landmark)[1])
        fvp.Z.value = np.inner(inverseHomogeneousMatrix(S(0,0)), landmark)[2]
        plug(w_M_c.jacobian, fvp.Jq)
        fvp.jacobian.recompute(T)
        print(np.matrix(fvp.jacobian.value)[0:2,0:6])

        l.signal(obsName + '_JfeatureReferencePosition').value = \
            matrixToTuple(dP(S(0, 0), landmark))
        l.signal(obsName + '_JsensorPosition').value = matrixToTuple(dS(0, 0))
        l.signal(obsName + '_weight').value = (1., 1.)

        l.signal(obsName + '_featureObservedPosition').value = \
            tuple(P(S(0,0), observed_landmark).tolist())
        l.signal(obsName + '_featureReferencePosition').value = \
            tuple(P(S(0,0), landmark).tolist())
        # Select (x, y, yaw) only!
        l.signal(obsName + '_correctedDofs').value = correctedDofs

        print("\n")

    l.configurationOffset.recompute(T)
    return (.5 * (xr - x) * (xr - x) + .5 * (yr - y) * (yr - y),)

def dS((x, y, theta), sensor):
    return ((1, 0, r * cos(sensorPos[sensor] + theta)),
            (0, 1, r * sin(sensorPos[sensor] + theta)))

def dP((x, y), l):
    (xr, yr) = features[l]
    return ((x - xr, y - yr,), )

for sensor in xrange(len(sensorPos)):
    for i in xrange(len(features)):
        prefix = 'obs_{0}_{1}'.format(sensor, i)
        l.add_landmark_observation(prefix)

        l.signal(prefix + '_JfeatureReferencePosition').value = dP(S(expectedRobot, sensor), i)
        l.signal(prefix + '_JsensorPosition').value = dS(expectedRobot, sensor)
        l.signal(prefix + '_weight').value = (1.,)

        l.signal(prefix + '_featureObservedPosition').value =  P(S(realRobot, sensor), i)
        l.signal(prefix + '_featureReferencePosition').value = \
            P(S(expectedRobot, sensor), i)

        l.signal(prefix + '_correctedDofs').value = \
            (1., 1.,  1.)


l.configurationOffset.recompute(0)


for sensor in xrange(len(sensorPos)):