1st order Taylor expansion and matrix

[marvin117]

I'm working on a LabVIEW module which determines the position of every Tag object (player) on a field. 

This is firstly done by calculating the operating point ('werkpunt') for every player, which means guessing where it will be next based on its heading, velocity and previous positions. So I have a collection of said operating points. 

The goal is the VI 'Estimate All Positions - Swarm Method' is to estimate all positions of Tags through linearization of the function f(xp, yp, xq, yq) = (xq-xp)^2 + (yq-yp)^2 = d_pq² between two Tags using 1st order Taylor expansion on (ap, bp, aq, bq) which are the two operating points for Tags p and q. I have calculated the first order derivative of this function to be -2.(a_q-a_p).x_p - 2.(b_q-b_p)y_p + 2.(a_q-a_p).x_q + 2.(b_q-b_p).y_q = d_pq² - (2.(a_q-a_p).a_p + 2.(b_q-b_p)_.b_p - 2.(a_q-a_p).a_q - 2((b_q-b_p).b_q + (a_q-a_p)² + (b_q-b_p)²)) where d_pq² is the measured distance between Tag p and q.

As you will see in the provided VI I'm using Solve Linear Equation for an input matrix on the left-hand side of the equation and a known vector which are the constants on the right-hand side of the equation. 

The second part of the code includes a simular calcuation from Anchors, which are on static points next to a field with a known xy-position, to Tags. Because the positions of Anchors are already known the formerly unknown variables are known and thus moved to the right-hand constant of the equation, so we get: 2(a_q – a_p).x_q + 2(b_q – b_p).y_q = d_pq² - (2.(a_q - a_p).a_p + 2.(b_q - b_p).b_p - 2.(a_q - a_p).a_q - 2((b_q - b_p).b_q+ (a_q - a_p)² + (b_q - b_p)²) - (-2.(a_q - a_p).x_pb - 2.(b_q - b_p). y_pb)) where x_pb and y_pb are the known points of an Anchor. These are added to the input matrix and known vector as well.

However, if I run the VI I don't get the expected result I desire with totally wrong estimations.

My question is as follows: is there a better solution to my problem such as using the nth Derivative of Polynomial VI?? What am I doing wrong?

I'm new to LabVIEW 🙂

[Bob_Schor]

It is very unclear to me what you want to do.  Don't tell me how you want to do it (i.e. "linearization of whatever"), but rather help me understand what is going on.  I know that somehow it involves points P and Q on an X-Y plane -- are these two separate points, or the same point at two different times?  What are you trying to compute about P and Q?  You mention heading and velocity -- are you trying to predict where P and Q will be (or will be relative to each other) at some (specified?) time in the future?

You mention "swarm".  Am I to gather that there are more than two points (P and Q)?  Are you interested in all pair-wise interactions?  Again, what are you trying to do?

I think that if the what is clearer, the how might suggest itself.  A critical point (oh, that could almost become a pun ...) is to understand the nature of P and Q, and how they are represented.

In any case, it is not at all clear to me that the strange math you posted is appropriate nor necessary, but then I don't yet understand the "what" of this post.

Bob Schor