For anyone who is interested in the formulas of automatic TMA:
First I set three unknown variables,
1. target distance at first bearing as "L1_distance"
2. target course as "crs"
3. target speed as "spd"
Let ownship coordinate be (m,n). Y axis points to north (0 deg), and X axis points to east (90 deg). All angles start at Y axis and rotate in clockwise direction. So we can get:
1. target speed component on x axis "u" = spd * sin(crs)
2. target speed component on y axis "v" = spd * cos(crs)
3. target coordinate (a,b) at first bearing = m1+L1_distance*sin(brg1), n1+L1_distance*cos(brg1)
4. target coordinate (x,y) at time t = (a+u*t, b+v*t)
5. the vector pointing from ownship to target = (x-m, y-n)
By knowing the bearing of this vector, we can get the bearing vector equation:
(y-n)*sin(brg) = (x-m)*cos(brg)
Since we have three unknown variables to solve, in order to make sure the left hand side of the equation is really close to the right hand side, we can use the least square method:
Let formula f(t) = (y-n)*sin(brg) - (x-m)*cos(brg)
So we need to minimise the sum of squared f(t) for all bearing vectors. I used BFGS algorithm to solve this utilising C++ Dlib library.
|