I am working with IMU data (accelerometer, magnetometer and gyroscope) in order to estimate the path done by the object that contained the IMU. I am using dead reckoning for this. I am trying to estimate the path done only using the initial coordinates in latitude and longitude. However i have been now giving the last coordinate of the real path. I was hoping that knowing the last coordinate I could help adjust my estimation so that it ends on the last coordinate but I am not sure how to do that. I am using the Madgwick filter in order to obtain a quaternion and from there obtain the yaw. With the yaw I can then get the new position using the formula bellow:
Formula: φ2 = asin( sin φ1 ⋅ cos δ + cos φ1 ⋅ sin δ ⋅ cos θ ) λ2 = λ1
- atan2( sin θ ⋅ sin δ ⋅ cos φ1, cos δ − sin φ1 ⋅ sin φ2 ) where φ is latitude, λ is longitude, θ is the yaw (clockwise from north), δ is the angular distance d/R; d being the distance travelled, R the earth’s radius
How can I implement now the last position to better my result and make sure that my estimated positions end up around the last coordinate? Is it possible? I though about calculating the bearing between the initial coordinate and the last coordinate and use that bearing to adjust the calculated yaw. Something like:
Formula for bearing: θ = atan2( sin Δλ ⋅ cos φ2 , cos φ1 ⋅ sin φ2 − sin φ1 ⋅ cos φ2 ⋅ cos Δλ ) where φ1,λ1 is the start point, φ2,λ2 the end point (Δλ is the difference in longitude)
and then adjust the yaw obtained from the quaternion by summing the bearing (in radians)
But it didn't work. I also thought about calculating the bearing between each estimated value and the last coordinate and just go updating it to then adjust the yaw value. However when I implement this the results are not the ones I expect. I am not a mathematician so bearing and yaw angles are kinda messing my head here. Any idea of the best way to do this?
EDIT: Sharing this image in the hopes it becomes clearer. [![enter image description here][1]][1] Ignore the red dots. But the orange line is the real path done with the IMU and the blue line is the estimated path using dead reckoning and the initial coordinate. What I wish to do is, knowing now the last real point, how can i make sure that my estimated last point does not end there but it adjusts so it ends in the last real point. I put the black line to kinda represent the direction i want my estimation to go. What I tried put a straight line for the end coordinate but that is not what I want.
[![enter image description here][2]][2]