I have a set of vectors (length of 50, essentially a set of curves) that i want to try to match another single curve(vector) and obtain the coefficients of each of the vectors in the first set to match the second curve. The coefficients need to be >= 0.0 . I.e, a linear combination of the first set of curves to match the single curve. Any help in which direction I should go would be helpful.
If I understand correctly, you have a set of curves
each of which you want to multiply with a scaling factor, so that it reproduces some target curve
as closely as possible.
This is easily done with a linear least squares approximation.
%# create some sample curves
x = -10:0.1:10;
g1 = exp(-(x-3).^2/4);
g2 = exp(-(x-0).^2/4);
g3 = exp(-(x+2).^2/4);
%# make a target curve, corrupt with noise
y = 2*g1+4*g2+g3+randn(size(x))*0.2;
%# use the `ldivide` operator to solve an equation of the form
%# A*x=B
%# so that x (=fact here) is x=A^-1*B or, in Matlab terms, A\B
%# note the transposes, A should be a n-by-3 array, B a n-by-1 array
%# so that x is a 3-by-1 array of factors
fact = [g1;g2;g3]'\y'
fact =
1.9524
3.9978
1.0105
%# Show the result
figure,plot(x,y)
hold on,plot(x,fact(1)*g1+fact(2)*g2+fact(3)*g3,'m')
so thats what he meant!..
mathematica version..
x = Table[i, {i, -10, 10, .1}];
basis = {
Exp[-(# - 3)^2/4] & /# x,
Exp[-(# - 0)^2/4] & /# x,
Exp[-(# + 2)^2/4] & /# x
};
Show[
ListPlot[Table[{x[[i]], #[[i]]}, {i, Length[x]}] ,
Joined -> True , PlotStyle -> Hue [Random[]]] & /# basis ]
y = Table [ 2 basis[[1, i]] + 4 basis[[2, i]] + basis[[3, i]] +
RandomReal[{.5, .5}] ,{i, Length[x]}];
dataplot = ListPlot[Table[{x[[i]], y[[i]]}, {i, Length[x]}] ]
mathematica does not magically do least squares if you simply solve an underdetermined system, so find a least squres result explicitly:
coefs = FindMinimum[
Total[(#^2 & /# (Sum[a[k] basis[[k]] , {k, Length[basis]}]-y) )],
Array[a, Length[basis]]][[2]]
Show[dataplot,
ListPlot[i = 0; {x[[++i]], #} & /#
(Sum[a[k] basis[[k]] , {k, 3}] /. coefs),
Joined -> True]]
note if you want ot restrict the coefficents to be >= 0 as stated you can simply square the values in the formulation like this:
coefs = FindMinimum[
Total[(#^2 & /#
(Sum[a[k]^2 basis[[k]] , {k, Length[basis]}]-y) )],
Array[a, Length[basis]]][[2]]
Show[dataplot,
ListPlot[i = 0; {x[[++i]], #} & /#
(Sum[a[k]^2 basis[[k]] , {k, 3}] /. coefs),
Joined -> True]]
you will get predictably poor results if the actual best fit wants to have a negative value.
Related
I'm new to Matlab and want to write a program that chooses the value of a parameter (P) to minimize the difference between two vectors, where each vector is a variable in a dataframe. The first vector (call it A) is a predetermined vector of 1s and 0s, and the second vector (call it B) has each of its entries determined as an indicator function that depends on the value of the parameter P and other variables in the dataframe. For instance, let C be a third variable in the dataset, so
A = [1, 0, 0, 1, 0]
B = [x, y, z, u, v]
where x = 1 if (C[1]+10)^0.5 - P > (C[1])^0.5 and otherwise x = 0, and similarly, y = 1 if (C[2]+10)^0.5 - P > (C[2])^0.5 and otherwise y = 0, and so on.
I'm not really sure where to start with the code, except that it might be useful to use the fminsearch command. Any suggestions?
Edit: I changed the above by raising to a power, which is closer to the actual example that I have. I'm also providing a complete example in response to a comment:
Let A be as above, and let C = [10, 1, 100, 1000, 1]. Then my goal with the Matlab code would be to choose a value of P to minimize the differences between the coordinates of the vectors A and B, where B[1] = 1 if (10+10)^0.5 - P > (10)^0.5 and otherwise B[1] = 0, and similarly B[2] = 1 if (1+10)^0.5 - P > (1)^0.5 and otherwise B[2] = 0, etc. So I want to choose P to maximize the likelihood that A[1] = B[1], A[2] = B[2], etc.
I have the following setup in Matlab, where ds is the name of my dataset:
ds.B = zeros(size(ds,1),1); % empty vector to fill
for i = 1:size(ds,1)
if ((ds.C(i) + 10)^(0.5) - P > (ds.C(i))^(0.5))
ds.B(i) = 1;
else
ds.B(i) = 0;
end
end
Now I want to choose the value of P to minimize the difference between A and B. How can I do this?
EDIT: I'm also wondering how to do this when the inequality is something like (C[i]+10)^0.5 - P*D[i] > (C[i])^0.5, where D is another variable in my dataset. Now P is a scalar being multiplied rather than just added. This seems more complicated since I can't solve for P exactly. How can I solve the problem in this case?
EDIT 1: It seems fminbnd() isn't optimal, likely due to the stairstep nature of the indicator function. I've updated to test the midpoints of all the regions between indicator function flips, plus endpoints.
EDIT 2: Updated to include dataset D as a coefficient of P.
If you can package your distance calculation up in a single function based on P, you can then search for its minimum.
arraySize = 1000;
ds.A = double(rand([arraySize,1]) > 0.5);
ds.C = rand(size(ds.A));
ds.D = rand(size(ds.A));
B = #(P)double((ds.C+10).^0.5 - P.*ds.D > ds.C.^0.5);
costFcn = #(P)sqrt(sum((ds.A-B(P)).^2));
% Solving the equation (C+10)^0.5 - P*D = C^0.5 for P, and sorting the results
BCrossingPoints = sort(((ds.C+10).^0.5-ds.C.^0.5)./ds.D);
% Taking the average of each crossing point with its neighbors
BMidpoints = (BCrossingPoints(1:end-1)+BCrossingPoints(2:end))/2;
% Appending endpoints onto the midpoints
PsToTest = [BCrossingPoints(1)-0.1; BMidpoints; BCrossingPoints(end)+0.1];
% Calculate the distance from A to B at each P to test
costResult = arrayfun(costFcn,PsToTest);
% Find the minimum cost
[~,lowestCostIndex] = min(costResult);
% Find the optimum P
optimumP = PsToTest(lowestCostIndex);
ds.B = B(optimumP);
semilogx(PsToTest,costResult)
xlabel('P')
ylabel('Distance from A to B')
1.- x is assumed positive real only, because with x<0 then complex values show up.
Since no comment is made in the question it seems reasonable to assume x real and x>0 only.
As requested, P 'the parameter' a scalar, P only has 2 significant states >0 or <0, let's see how is this:
2.- The following lines generate kind-of random A and C.
Then a sweep of p is carried out and distances d1 and d2 are calculated.
d1 is euclidean distance and d2 is the absolute of the difference between A and and B converting both from binary to decimal:
N=10
% A=[1 0 0 1 0]
A=randi([0 1],1,N);
% C=[10 1 1e2 1e3 1]
C=randi([0 1e3],1,N)
p=[-1e4:1:1e4]; % parameter to optimize
B=zeros(1,numel(A));
d1=zeros(1,numel(p)); % euclidean distance
d2=zeros(1,numel(p)); % difference distance
for k1=1:1:numel(p)
B=(C+10).^.5-p(k1)>C.^.5;
d1(k1)=(sum((B-A).^2))^.5;
d2(k1)=abs(sum(A.*2.^[numel(A)-1:-1:0])-sum(B.*2.^[numel(A)-1:-1:0]));
end
figure;
plot(p,d1)
grid on
xlabel('p');title('d1')
figure
plot(p,d2)
grid on
xlabel('p');title('d2')
The only degree of freedom to optimise seems to be the sign of P regardless of |P| value.
3.- f(p,x) has either no root, or just one root, depending upon p
The threshold funtion is
if f(x)>0 then B(k)==1 else B(k)==0
this is
f(p,x)=(x+10)^.5-p-x^.5
Now
(x+10).^.5-p>x.^.5 is same as (x+10).^.5-x.^.5>p
There's a range of p that keeps f(p,x)=0 without any (real) root.
For the particular case p=0 then (x+10).^.5 and x.^.5 do not intersect (until Inf reached = there's no intersection)
figure;plot(x,(x+10).^.5,x,x.^.5);grid on
[![enter image description here][3]][3]
y2=diff((x+10).^.5-x.^.5)
figure;plot(x(2:end),y2);
grid on;xlabel('x')
title('y2=diff((x+10).^.5-x.^.5)')
[![enter image description here][3]][3]
% 005
This means the condition f(x)>0 is always true holding all bits of B=1. With B=1 then d(A,B) turns into d(A,1), a constant.
However, for a certain value of p then there's one root and f(x)>0 is always false keeping all bits of B=0.
In this case d(A,B) the cost function turns into d(A,0) and this is A itself.
4.- P as a vector
The optimization gains in degrees of freedom if instead of P scalar, P is considered as vector.
For a given x there's a value of p that switches B(k) from 0 to 1.
Any value of p below such threshold keeps B(k)=0.
Equivalently, inverting f(x) :
g(p)=(10-p^2)^2/(4*p^2)>x
Values of x below this threshold bring B closer to A because for each element of B it's flipped to the element value of A.
Therefore, it's convenient to consider P as a vector, not a ascalar, and :
For all, or as many (as possible) elements of C to meet c(k)<(10-p^2)^2/(4*p^2) in order to get C=A or
minimize d(A,C)
5.- roots of f(p,x)
syms t positive
p=[-1000:.1:1000];
zp=NaN*ones(1,numel(p));
sol=zeros(1,numel(p));
for k1=1:1:numel(p)
p(k1)
eq1=(t+10)^.5-p(k1)-t^.5-p(k1)==0;
s1=solve(eq1,t);
if ~isempty(s1)
zp(k1)=s1;
end
end
nzp=~isnan(zp);
zp(nzp)
returns
=
620.0100 151.2900 64.5344 34.2225 20.2500 12.7211
8.2451 5.4056 3.5260 2.2500 1.3753 0.7803
0.3882 0.1488 0.0278
I have a state space system with matrices A,B,C and D.
I can either create a state space system, sys1 = ss(A,B,C,D), of it or compute the transfer function matrix, sys2 = C*inv(z*I - A)*B + D
However when I draw the bode plot of both systems, they are different while they should be the same.
What is going wrong here? Does anyone have a clue? I know btw that the bodeplot generated by sys1 is correct.
The system can be downloaded here: https://dl.dropboxusercontent.com/u/20782274/system.mat
clear all;
close all;
clc;
Ts = 0.01;
z = tf('z',Ts);
% Discrete system
A = [0 1 0; 0 0 1; 0.41 -1.21 1.8];
B = [0; 0; 0.01];
C = [7 -73 170];
D = 1;
% Set as state space
sys1 = ss(A,B,C,D,Ts);
% Compute transfer function
sys2 = C*inv(z*eye(3) - A)*B + D;
% Compute the actual transfer function
[num,den] = ss2tf(A,B,C,D);
sys3 = tf(num,den,Ts);
% Show bode
bode(sys1,'b',sys2,'r--',sys3,'g--');
Edit: I made a small mistake, the transfer function matrix is sys2 = C*inv(z*I - A)*B + D, instead of sys2 = C*inv(z*I - A)*B - D which I did wrote done before. The problem still holds.
Edit 2: I have noticted that when I compute the denominator, it is correct.
syms z;
collect(det(z*eye(3) - A),z)
Your assumption that sys2 = C*inv(z*I- A)*B + D is incorrect. The correct equivalent to your state-space system (A,B,C,D) is sys2 = C*inv(s*I- A)*B + D. If you want to express it in terms of z, you'll need to invert the relationship z = exp(s*T). sys1 is the correct representation of your state-space system. What I would suggest for sys2 is to do as follows:
sys1 = ss(mjlsCE.A,mjlsCE.B,mjlsCE.C,mjlsCE.D,Ts);
sys1_c = d2c(sys1);
s = tf('s');
sys2_c = sys1_c.C*inv(s*eye(length(sys1_c.A)) - sys1_c.A)*sys1_c.B + sys1_c.D;
sys2_d = c2d(sys2_c,Ts);
That should give you the correct result.
Due to inacurracy of the inverse function extra unobservable poles and zeros are added to the system. For this reason you need to compute the minimal realization of your transfer function matrix.
Meaning
% Compute transfer function
sys2 = minreal(C*inv(z*eye(3) - A)*B + D);
What you are noticing is actually a numerical instability regarding pole-zero pair cancellations.
If you run the following code:
A = [0, 1, 0; 0, 0, 1; 0.41, -1.21, 1.8] ;
B = [0; 0; 0.01] ;
C = [7, -73, 170] ;
D = 1 ;
sys_ss = ss(A, B, C, D) ;
sys_tf_simp = tf(sys_ss) ;
s = tf('s') ;
sys_tf_full = tf(C*inv(s*eye(3) - A)*B + D) ;
zero(sys_tf_simp)
zero(sys_tf_full)
pole(sys_tf_simp)
pole(sys_tf_full)
you will see that the transfer function formulated by matrices directly has a lot more poles and zeros than the one formulated by MatLab's tf function. You will also notice that every single pair of these "extra" poles and zeros are equal- meaning that they cancel with each other if you were to simply the rational expression. MatLab's tf presents the simplified form, with equal pole-zero pairs cancelled out. This is algebraically equivalent to the unsimplified form, but not numerically.
When you call bode on the unsimplified transfer function, MatLab begins its numerical plotting routine with the pole-zero pairs not cancelled algebraically. If the computer was perfect, the result would be the same as in the simplified case. However, numerical error when evaluating the numerator and denominators effectively leaves some of the pole-zero pairs "uncancelled" and as many of these poles are in the far right side of the s plane, they drastically influence the output behavior.
Check out this link for info on this same problem but from the perspective of design: http://ctms.engin.umich.edu/CTMS/index.php?aux=Extras_PZ
In your original code, you can think of the output drawn in green as what the naive designer wanted to see when he cancelled all his unstable poles with zeros, but the output drawn in red is what he actually got because in practice, finite-precision and real-world tolerances prevent the poles and zeros from cancelling perfectly.
Why is an unobservable / uncontrollable pole? I think this issue comes only because the inverse of a transfer function matrix is inaccurate in Matlab.
Note:
A is 3x3 and the minimal realization has also order 3.
What you did is the inverse of a transfer function matrix, not a symbolic or numeric matrix.
# Discrete system
Ts = 0.01;
A = [0 1 0; 0 0 1; 0.41 -1.21 1.8];
B = [0; 0; 0.01];
C = [7 -73 170];
D = 1;
z = tf('z', Ts)) # z is a discrete tf
A1 = z*eye(3) - A # a tf matrix with a direct feedthrough matrix A
# inverse it, multiply with C and B from left and right, and plus D
G = D + C*inv(A1)*B
G is now a scalar (SISO) transfer function.
Without "minreal", G has order 9 (funny, I don't know how Matlab computes it, perhaps the "Adj(.)/det(.)" method). Matlab cannot cancel the common factors in the numerator and the denominator, because z is of class 'tf' rather than a symbolic variable.
Do you agree or do I have misunderstanding?
I have 3 matrices x, y and z of order 3*3. I want to create a new matrix k with value = 1./(x.^2+y.^2+z.^2) if (x.^2+y.^2+z.^2 > 1) and value = 0 otherwise.
I am trying to use this :
k(x.^2+y.^2+z.^2>1)= 1./(x.^2+y.^2+z.^2)
but it gives error : In an assignment A(I) = B, the number of elements in B and I must be the same.
Can anyone provide a simple solution in a single line where I don't need to use for loops
I am not sure why you'd want to do this as opposed to splitting it up into two operations. This way, you save the cost of computing the sum of squares twice.
x = rand(3,3);
y = rand(3,3);
z = rand(3,3);
k = 1./(x.^2+y.^2+z.^2);
k(k>1)=0;
In any case, another way to do it would be using principles of Functional Programming:
x = rand(3,3);
y = rand(3,3);
z = rand(3,3);
myfun = #(x,y,z) 1/(x^2+y^2+z^2) * (x^2+y^2+z^2>1);
k = arrayfun(myfun, x, y, z);
Alternately, you can mix everything into one line as:
k = arrayfun(#(x,y,z) 1/(x^2+y^2+z^2) * (x^2+y^2+z^2>1), x, y, z);
What this code does is maps the function myfun to each of the data elements. The function myfun is quite simple. It computes the required quantity but multiplies it with the binding condition. However, you might want to beware.
EDIT: To address the comment.
If you don't want to compute the quantity at all, we can use conditional anonymous functions. For more details, you can refer to this guide.
iif = #(varargin) varargin{2 * find([varargin{1:2:end}], 1, 'first')}();
myfun = #(x,y,z) iif( x^2+y^2+z^2 <= 1, #() 0, x^2+y^2+z^2>1 ,#() 1/(x^2+y^2+z^2));
k = arrayfun(myfun, x, y, z);
How about
k = x.^2+y.^2+z.^2;
k(k < 1) = 0;
k(k~= 0) = 1 ./ k(k~=0);
If you are trying to save some processing time (i.e. do not compute at all the sum of squares for those cases when it is less than one) then pretty much the only solution is a table lookup
Otherwise the following code should work
k=1./(x.^2+y.^2+z.^2)
k(k<=1)=0
you can cut some time (assuming x, y and z could be greater than 1)
idx0=x<1 & y<1 & z<1
k=zeros(3)
k(idx0)=1./(x(idx0).^2+y(idx0).^2+z(idx0)^2)
k(k<=1)=0
Your original solution will work if you change it to use an indexer (I haven't profiled it, but I am pretty sure it will take longer, than mine :) )
idx0=x.^2+y.^2+z.^2>1
k=zeros(3)
k(idx0)=1./(x(idx0).^2+y(idx0).^2+z(idx0)^2)
I have a logical matrix:
0,0,0,0,0,0,0,0,0,0,0
0,0,0,0,0,0,0,0,0,0,0
0,0,0,1,1,1,1,1,0,0,0
0,0,0,1,1,1,1,1,1,0,0
0,0,1,1,1,1,1,1,1,0,0
0,0,1,1,1,1,1,1,1,0,0
0,0,1,1,1,1,1,1,1,0,0
0,0,0,1,1,1,1,1,1,0,0
0,0,0,0,1,1,0,0,0,0,0
0,0,0,0,0,0,0,0,0,0,0
0,0,0,0,0,0,0,0,0,0,0
and I would like to fit an ellipse as best as possible and calculate the error. The error could be the elementwise difference between the original data and the best ellipse found.
I have seen the following method: MATLAB Curve Fitting (Ellipse-like)
but I am not sure that is the shortest way to do it.
How would you recommend to find the closest elliptical logical matrix?
Find the ellipse 6 DOF parameters:
%get edge locations
md = m-imerode(m,ones(3));
md = padarray(md,10);axis equal
%find edges x,y
[y,x] = find(md);
%fit to Q = a*x^2 + b*y^2 + 2*c*x*y + 2*d*x + 2*e*y + f
H=[x.*x y.*y 2*x.*y x y x*0+1];
[v,g]=eig(H'*H);
a = v(1,1);
b = v(2,1);
c = v(3,1);
d = v(4,1);
e = v(5,1);
f = v(6,1);
[xg, yg] = meshgrid(1:size(md,2),1:size(md,1));
ev = a*xg.^2+b*yg.^2+2*c*xg.*yg+d*xg+e*yg+f;
imagesc(ev);axis equal
I would fist extract the boundary of your logical ellipse. Iterate the following function from i=2 to n-1, j=2 to m-1, for [n,m]=size(M). It will give you the "boundary matrix", where the boundary is represented by 1 and 0 for others. Then use the method described in here to get the coefficients of the ellipse equation. Note that the matrix indices start from top-left. So you may need to modify the index ordering depending on where you want the origin to be.
function [ bd ] = logical_neighbor( loM, i, j )
bd=0;
if loM(i,j) == 0
return;
else
for ni=1:3
for nj=1:3
if loM(i-2+ni,j-2+nj) == 0
bd= 1;
return;
end
end
end
end
end
I am trying to determine the (x,y,z) coordinates of a point p. What I have are the distances to 4 different points m1, m2, m3, m4 with known coordinates.
In detail: what I have is the coordinates of 4 points (m1,m2,m3,m4) and they are not in the same plane:
m1: (x1,y1,z1),
m2: (x2,y2,z2),
m3: (x3,y3,z3),
m4: (x4,y4,z4)
and the Euclidean distances form m1->p, m2->p, m3->p and m4->p which are
D1 = sqrt( (x-x1)^2 + (y-y1)^2 + (z-z1)^2);
D2 = sqrt( (x-x2)^2 + (y-y2)^2 + (z-z2)^2);
D3 = sqrt( (x-x3)^2 + (y-y3)^2 + (z-z3)^2);
D4 = sqrt( (x-x4)^2 + (y-y4)^2 + (z-z4)^2);
I am looking for (x,y,z). I tried to solve this non-linear system of 4 equations and 3 unknowns with matlab fsolve by taking the euclidean distances but didn't manage.
There are two questions:
How can I find the unknown coordinates of point p: (x,y,z)
What is the minimum number of points m with known coordinates and
distances to p that I need in order to find (x,y,z)?
EDIT:
Here is a piece of code that gives no solutions:
Lets say that the points I have are:
m1 = [ 370; 1810; 863];
m2 = [1586; 185; 1580];
m3 = [1284; 1948; 348];
m4 = [1732; 1674; 1974];
x = cat(2,m1,m2,m3,m4)';
And the distance from each point to p are
d = [1387.5; 1532.5; 1104.7; 0855.6]
From what I understood if I want to run fsolve I have to use the following:
1. Create a function
2. Call fsolve
function F = calculateED(p)
m1 = [ 370; 1810; 863];
m2 = [1586; 185; 1580];
m3 = [1284; 1948; 348];
m4 = [1732; 1674; 1974];
x = cat(2,m1,m2,m3,m4)';
d = [1387.5; 1532.5; 1104.7; 0855.6]
F = [d(1,1)^2 - (p(1)-x(1,1))^2 - (p(2)-x(1,2))^2 - (p(3)-x(1,3))^2;
d(2,1)^2 - (p(1)-x(2,1))^2 - (p(2)-x(2,2))^2 - (p(3)-x(2,3))^2;
d(3,1)^2 - (p(1)-x(3,1))^2 - (p(2)-x(3,2))^2 - (p(3)-x(3,3))^2;
d(4,1)^2 - (p(1)-x(4,1))^2 - (p(2)-x(4,2))^2 - (p(3)-x(4,3))^2;];
and then call fsolve:
p0 = [1500,1500,1189]; % initial guess
options = optimset('Algorithm',{'levenberg-marquardt',.001},'Display','iter','TolX',1e-1);
[p,Fval,exitflag] = fsolve(#calculateED,p0,options);
I am running Matlab 2011b.
Am I missing something?
How would the least squares solution be?
One note here is that m1, m2, m3, m4 and d values may not be given accurately but for an analytical solution that shouldn't be a problem.
mathematica readily numericall solves the three point problem:
p = Table[ RandomReal[{-1, 1}, {3}], {3}]
r = RandomReal[{1, 2}, {3}]
Reduce[Simplify[ Table[Norm[{x, y, z} - p[[i]]] == r[[i]] , {i, 3}],
Assumptions -> {Element[x | y | z, Reals]}], {x, y, z}, Reals]
This will typically return false as random spheres will typically not have triple intersection points.
When you have a solution you'll typically have a pair like this..
(* (x == -0.218969 && y == -0.760452 && z == -0.136958) ||
(x == 0.725312 && y == 0.466006 && z == -0.290347) *)
This somewhat surprisingly has a failrly elegent analytic solution. Its a bit involved so I'll wait to see if someone has it handy and if not and there is interest I'll try to remember the steps..
Edit, approximate solution following Dmitys least squares suggestion:
p = {{370, 1810, 863}, {1586, 185, 1580}, {1284, 1948, 348}, {1732,
1674, 1974}};
r = {1387.5, 1532.5, 1104.7, 0855.6};
solution = {x, y, z} /.
Last#FindMinimum[
Sum[(Norm[{x, y, z} - p[[i]]] - r[[i]] )^2, {i, 1, 4}] , {x, y, z}]
Table[ Norm[ solution - p[[i]]], {i, 4}]
As you see you are pretty far from exact..
(* solution point {1761.3, 1624.18, 1178.65} *)
(* solution radii: {1438.71, 1504.34, 1011.26, 797.446} *)
I'll answer the second question. Let's name the unknown point X. If you have only known point A and know the distance form X to A then X can be on a sphere with the center in A.
If you have two points A,B then X is on a circle given by the intersection of the spheres with centers in A and B (if they intersect that is).
A third point will add another sphere and the final intersection between the three spheres will give two points.
The fourth point will finaly decide which of those two points you're looking for.
This is how GPS actually works. You have to have at least three satellites. Then the GPS will guess which of the two points is the correct one, since the other one is in space, but it won't be able to tell you the altitude. Technically it should, but there are also errors, so the more satellites you "see" the less the error.
Have found this question which might be a starting point.
Take first three equation and solve i for 3 equation and 3 variables in MATLAB. After solving the equation you will get two pairs of values or we can say two set of coordinates of p.
keep each set in the 4th equation and you can find out the set which satisfies the equation is the answer