An autonomous vehicle comprising: at least one image sensor to provide measurements of landmark position for a plurality of landmarks; and processing functionality to estimate the position of the plurality of landmarks in a global frame and in the autonomous vehicle's frame, and to estimate the kinematic state of the autonomous vehicle in a global frame based, at least in part, on the measurements of landmark position from the at least one image sensor; characterised in that the processing functionality is further configured to calculate errors in the estimated positions of the plurality of landmarks in the global frame and in the estimate of the kinematic state of the autonomous vehicle in the global frame by using a plurality of unit projection vectors between the estimated positions of the plurality landmarks in the autonomous vehicle's frame and a plurality of unit projection vectors between the estimated positions of the plurality of landmarks in the global frame; wherein to calculate the errors, the processing functionality is further configured to: form a first plurality of unit projection vectors between landmark positions in the global frame for at least three landmarks; form a second plurality of unit projection vectors between landmark positions in the vehicle frame for the same at least three landmarks; calculate heading angle drift errors in the estimate of the vehicle kinematic state of the autonomous vehicle and the estimate of the position of the plurality of landmarks in the global frame by solving a constrained optimization problem with the first and second plurality of unit projection vectors. at least one image sensor to provide measurements of landmark position for a plurality of landmarks; and processing functionality to estimate the position of the plurality of landmarks in a global frame and in the autonomous vehicle's frame, and to estimate the kinematic state of the autonomous vehicle in a global frame based, at least in part, on the measurements of landmark position from the at least one image sensor; characterised in that the processing functionality is further configured to calculate errors in the estimated positions of the plurality of landmarks in the global frame and in the estimate of the kinematic state of the autonomous vehicle in the global frame by using a plurality of unit projection vectors between the estimated positions of the plurality landmarks in the autonomous vehicle's frame and a plurality of unit projection vectors between the estimated positions of the plurality of landmarks in the global frame; wherein to calculate the errors, the processing functionality is further configured to: form a first plurality of unit projection vectors between landmark positions in the global frame for at least three landmarks; form a second plurality of unit projection vectors between landmark positions in the vehicle frame for the same at least three landmarks; calculate heading angle drift errors in the estimate of the vehicle kinematic state of the autonomous vehicle and the estimate of the position of the plurality of landmarks in the global frame by solving a constrained optimization problem with the first and second plurality of unit projection vectors. form a first plurality of unit projection vectors between landmark positions in the global frame for at least three landmarks; form a second plurality of unit projection vectors between landmark positions in the vehicle frame for the same at least three landmarks; calculate heading angle drift errors in the estimate of the vehicle kinematic state of the autonomous vehicle and the estimate of the position of the plurality of landmarks in the global frame by solving a constrained optimization problem with the first and second plurality of unit projection vectors.