Solving ODE in Scilab - ode

I am trying to do some electric circuit analysis in Scilab by solving an ODE. But I need to change an ODE depending on current value of the function. I have implemented the solution in Scala using RK4 method and it works perfectly. Now I am trying to do the same but using standard functions in Scilab. And it is not working. I have tried to solve these two ODEs seperately and it is OK.
clear
state = 0 // state 0 is charging, 1 is discharging
vb = 300.0; vt = 500.0;
r = 100.0; rd = 10.0;
vcc = 600;
c = 48.0e-6;
function dudx = curfunc(t, uu)
if uu < vb then state = 0
elseif uu > vt state = 1
end
select state
case 0 then // charging
dudx = (vcc - uu) / (r * c)
case 1 then // discharging
dudx = - uu / (rd * c) + (vcc - uu) / (r * c)
end
endfunction
y0 = 0
t0 = 0
t = 0:1e-6:10e-3
%ODEOPTIONS=[1, 0, 0, 1e-6, 1e-12, 2, 500, 12, 5, 0, -1, -1]
y = ode(y0, t0, t, 1e-3, 1e-6, curfunc)
clear %ODEOPTIONS
plot(t, y)
so here I am solving for a node voltage, if node voltage is exceeding top threshold (vt) then discharging ODE is used, if node voltage is going below the bottom voltage (vb) then charging ODE is used. I have tried to play with %ODEOPTIONS but no luck

you can also use the ode("root"...) option.
The code will ressemble to
clear
state = 0 // state 0 is charging, 1 is discharging
vb = 300.0; vt = 500.0;
r = 100.0; rd = 10.0;
vcc = 600;
c = 48.0e-6;
function dudx = charging(t, uu)
//uu<vt
dudx = (vcc - uu) / (r * c)
endfunction
function e=chargingssurf(t,uu)
e=uu-vt
endfunction
function dudx = discharging(t, uu)
//uu<vb
dudx = - uu / (rd * c) + (vcc - uu) / (r * c)
endfunction
function e=dischargingssurf(t,uu)
e=uu-vb
endfunction
y0 = 0
t0 = 0
t = 0:1e-6:10e-3
Y=[];
T=[];
[y,rt] = ode("root",y0, t0, t, 1e-3, 1e-6, charging,1,chargingssurf);
disp(rt)
k=find(t(1:$-1)<rt(1)&t(2:$)>=rt(1))
Y=[Y y];;
T=[T t(1:k) rt(1)];
[y,rt] = ode("root",y($), rd(1), t(k+1:$), 1e-3, 1e-6, discharging,1,dischargingssurf);
Y=[Y y];
T=[T t(k+1:$)];
plot(T, Y)
The discharging code seems to be wrong...

I think it boils down to a standard problem: The standard ODE solvers have step size control. In contrast to the fixed step size of RK4. For step size control to work, the ODE function needs to have differentiability at least of the same order as the order of the method. Jumps as in your function are thus extremely unfortunate.
Another point to consider is that the internal steps of the method are not always in increasing time, they may jump backwards. See the coefficient table for Dormand-Prince for an example. Thus event based model changes as in your problem may lead to strange effects if the first problem were circumvented.
Workaround (not really a solution)
Declare state as a global variable, since as a local variable the value gets always reset to the global constant 0.
function dudx = curfunc(t, uu)
global state
...
endfunction
...
y = ode("fix",y0, t0, t, rtol, atol, curfunc)

Related

Finite difference method for solving the Klein-Gordon equation in Matlab

I am trying to numerically solve the Klein-Gordon equation that can be found here. To make sure I solved it correctly, I am comparing it with an analytical solution that can be found on the same link. I am using the finite difference method and Matlab. The initial spatial conditions are known, not the initial time conditions.
I start off by initializing the constants and the space-time coordinate system:
close all
clear
clc
%% Constant parameters
A = 2;
B = 3;
lambda = 2;
mu = 3;
a = 4;
b = - (lambda^2 / a^2) + mu^2;
%% Coordinate system
number_of_discrete_time_steps = 300;
t = linspace(0, 2, number_of_discrete_time_steps);
dt = t(2) - t(1);
number_of_discrete_space_steps = 100;
x = transpose( linspace(0, 1, number_of_discrete_space_steps) );
dx = x(2) - x(1);
Next, I define and plot the analitical solution:
%% Analitical solution
Wa = cos(lambda * x) * ( A * cos(mu * t) + B * sin(mu * t) );
figure('Name', 'Analitical solution');
surface(t, x, Wa, 'edgecolor', 'none');
colormap(jet(256));
colorbar;
xlabel('t');
ylabel('x');
title('Wa(x, t) - analitical solution');
The plot of the analytical solution is shown here.
In the end, I define the initial spatial conditions, execute the finite difference method algorithm and plot the solution:
%% Numerical solution
Wn = zeros(number_of_discrete_space_steps, number_of_discrete_time_steps);
Wn(1, :) = Wa(1, :);
Wn(2, :) = Wa(2, :);
for j = 2 : (number_of_discrete_time_steps - 1)
for i = 2 : (number_of_discrete_space_steps - 1)
Wn(i + 1, j) = dx^2 / a^2 ...
* ( ( Wn(i, j + 1) - 2 * Wn(i, j) + Wn(i, j - 1) ) / dt^2 + b * Wn(i - 1, j - 1) ) ...
+ 2 * Wn(i, j) - Wn(i - 1, j);
end
end
figure('Name', 'Numerical solution');
surface(t, x, Wn, 'edgecolor', 'none');
colormap(jet(256));
colorbar;
xlabel('t');
ylabel('x');
title('Wn(x, t) - numerical solution');
The plot of the numerical solution is shown here.
The two plotted graphs are not the same, which is proof that I did something wrong in the algorithm. The problem is, I can't find the errors. Please help me find them.
To summarize, please help me change the code so that the two plotted graphs become approximately the same. Thank you for your time.
The finite difference discretization of w_tt = a^2 * w_xx - b*w is
( w(i,j+1) - 2*w(i,j) + w(i,j-1) ) / dt^2
= a^2 * ( w(i+1,j) - 2*w(i,j) + w(i-1,j) ) / dx^2 - b*w(i,j)
In your order this gives the recursion equation
w(i,j+1) = dt^2 * ( (a/dx)^2 * ( w(i+1,j) - 2*w(i,j) + w(i-1,j) ) - b*w(i,j) )
+2*w(i,j) - w(i,j-1)
The stability condition is that at least a*dt/dx < 1. For the present parameters this is not satisfied, they give this ratio as 2.6. Increasing the time discretization to 1000 points is sufficient.
Next up is the boundary conditions. Besides the two leading columns for times 0 and dt one also needs to set the values at the boundaries for x=0 and x=1. Copy also them from the exact solution.
Wn(:,1:2) = Wa(:,1:2);
Wn(1,:)=Wa(1,:);
Wn(end,:)=Wa(end,:);
Then also correct the definition (and use) of b to that in the source
b = - (lambda^2 * a^2) + mu^2;
and the resulting numerical image looks identical to the analytical image in the color plot. The difference plot confirms the closeness

Why does the code terminate with a "Solution Not Found" error and "EXIT: Converged to a point of local infeasibility. Problem may be infeasible"?

I cannot seem to figure out why IPOPT cannot find a solution to this. Initially, I thought the problem was totally infeasible but when I reduce the value of col_total to any number below 161000 or comment out the last constraint equation that contains col_total, it solves and EXITs with an Optimal Solution Found and a final objective value function of -161775.256826753. I have solved the same Maximization problem using Artificial Bee Colony and Particle Swamp Optimization techniques, and they solve and return optimal objective value function at least 225000 and 226000 respectively. Could it be that another solver is required? I have also tried APOPT, BPOPT, and IPOPT and have tinkered around with the tolerance values, but no combination none seems to work just yet. The code is posted below. Any guidance will be hugely appreciated.
from gekko import GEKKO
import numpy as np
distances = np.array([[[0, 0],[0,0],[0,0],[0,0]],\
[[155,0],[0,0],[0,0],[0,0]],\
[[310,0],[155,0],[0,0],[0,0]],\
[[465,0],[310,0],[155,0],[0,0]],\
[[620,0],[465,0],[310,0],[155,0]]])
alpha = 0.5 / np.log(30/0.075)
diam = 31
free = 7
rho = 1.2253
area = np.pi * (diam / 2)**2
min_v = 5.5
axi_max = 0.32485226746
col_total = 176542.96546512868
rat = 14
nn = 5
u_hub_lowerbound = 5.777777777777778
c_pow = 0.59230249
p_max = 0.5 * rho * area * c_pow * free**3
# Initialize Model
m = GEKKO(remote=True)
#initialize variables, Set lower and upper bounds
x = [m.Var(value = 0.03902278, lb = 0, ub = axi_max) \
for i in range(nn)]
# i = 0
b = 1
c = 0
v_s = list()
for i in range(nn-1): # Loop runs for nn-1 times
# print(i)
# print(i,b,c)
squared_defs = list()
while i < b:
d = distances[b][c][0]
r = distances[b][c][1]
ss = (2 * (alpha * d) / diam)
tt = r / ((diam/2) + (alpha * d))
squared_defs.append((2 * x[i] / (1 + ss**2)) * np.exp(-(tt**2)) ** 2)
i+=1
c+=1
#Equations
m.Equation((free * (1 - (sum(squared_defs))**0.5)) - rat <= 0)
m.Equation((free * (1 - (sum(squared_defs))**0.5)) - u_hub_lowerbound >= 0)
v_s.append(free * (1 - (sum(squared_defs))**0.5))
squared_defs.clear()
b+=1
c=0
# Inserts free as the first item on the v_s list to
# increase len(v_s) to nn, so that 'v_s' and 'x'
# are of same length
v_s.insert(0, free)
gamma = list()
for i in range(len(x)):
bet = (4*x[i]*((1-x[i])**2) * rho * area) / 2
gam = bet * v_s[i]**3
gamma.append(gam)
#Equations
m.Equation(x[i] - axi_max <= 0)
m.Equation((((4*x[i]*((1-x[i])**2) * rho * area) / 2) \
* v_s[i]**3) - p_max <= 0)
m.Equation((((4*x[i]*((1-x[i])**2) * rho * area) / 2) * \
v_s[i]**3) > 0)
#Equation
m.Equation(col_total - sum(gamma) <= 0)
#Objective
y = sum(gamma)
m.Maximize(y) # Maximize
#Set global options
m.options.IMODE = 3 #steady state optimization
#Solve simulation
m.options.SOLVER = 3
m.solver_options = ['linear_solver ma27','mu_strategy adaptive','max_iter 2500', 'tol 1.0e-5' ]
m.solve()
Built the equations without .value in the expressions. The x[i].value is only needed at the end to view the solution after the solution is complete or to initialize the value of x[i]. The expression m.Maximize(y) is more readable than m.Obj(-y) although they are equivalent.
from gekko import GEKKO
import numpy as np
distances = np.array([[[0, 0],[0,0],[0,0],[0,0]],\
[[155,0],[0,0],[0,0],[0,0]],\
[[310,0],[155,0],[0,0],[0,0]],\
[[465,0],[310,0],[155,0],[0,0]],\
[[620,0],[465,0],[310,0],[155,0]]])
alpha = 0.5 / np.log(30/0.075)
diam = 31
free = 7
rho = 1.2253
area = np.pi * (diam / 2)**2
min_v = 5.5
axi_max = 0.069262150781
col_total = 20000
p_max = 4000
rat = 14
nn = 5
# Initialize Model
m = GEKKO(remote=True)
#initialize variables, Set lower and upper bounds
x = [m.Var(value = 0.03902278, lb = 0, ub = axi_max) \
for i in range(nn)]
i = 0
b = 1
c = 0
v_s = list()
for turbs in range(nn-1): # Loop runs for nn-1 times
squared_defs = list()
while i < b:
d = distances[b][c][0]
r = distances[b][c][1]
ss = (2 * (alpha * d) / diam)
tt = r / ((diam/2) + (alpha * d))
squared_defs.append((2 * x[i] / (1 + ss**2)) \
* m.exp(-(tt**2)) ** 2)
i+=1
c+=1
#Equations
m.Equation((free * (1 - (sum(squared_defs))**0.5)) - rat <= 0)
m.Equation(min_v - (free * (1 - (sum(squared_defs))**0.5)) <= 0 )
v_s.append(free * (1 - (sum(squared_defs))**0.5))
squared_defs.clear()
b+=1
a=0
c=0
# Inserts free as the first item on the v_s list to
# increase len(v_s) to nn, so that 'v_s' and 'x'
# are of same length
v_s.insert(0, free)
beta = list()
gamma = list()
for i in range(len(x)):
bet = (4*x[i]*((1-x[i])**2) * rho * area) / 2
gam = bet * v_s[i]**3
#Equations
m.Equation((((4*x[i]*((1-x[i])**2) * rho * area) / 2) \
* v_s[i]**3) - p_max <= 0)
m.Equation((((4*x[i]*((1-x[i])**2) * rho * area) / 2) \
* v_s[i]**3) > 0)
gamma.append(gam)
#Equation
m.Equation(col_total - sum(gamma) <= 0)
#Objective
y = sum(gamma)
m.Maximize(y) # Maximize
#Set global options
m.options.IMODE = 3 #steady state optimization
#Solve simulation
m.options.SOLVER = 3
m.solve()
This gives a successful solution with maximized objective 20,000:
Number of Iterations....: 12
(scaled) (unscaled)
Objective...............: -4.7394814741924645e+00 -1.9999999999929641e+04
Dual infeasibility......: 4.4698510326511536e-07 1.8862194343304290e-03
Constraint violation....: 3.8275766582203308e-11 1.2941979026166479e-07
Complementarity.........: 2.1543608536533588e-09 9.0911246952931704e-06
Overall NLP error.......: 4.6245685940749926e-10 1.8862194343304290e-03
Number of objective function evaluations = 80
Number of objective gradient evaluations = 13
Number of equality constraint evaluations = 80
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 13
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 12
Total CPU secs in IPOPT (w/o function evaluations) = 0.010
Total CPU secs in NLP function evaluations = 0.011
EXIT: Optimal Solution Found.
The solution was found.
The final value of the objective function is -19999.9999999296
---------------------------------------------------
Solver : IPOPT (v3.12)
Solution time : 3.210000000399305E-002 sec
Objective : -19999.9999999296
Successful solution
---------------------------------------------------

Matlab visualization

I used this link to walk through FDTD code and write it for myself to practice.
https://www.youtube.com/watch?v=y7hJAhKp2d8
This is the code that I wrote (its not verbatim, but extremely similar). When I run the program, I am told that "Field amplitude is too small to visualize properly." I don't understand why. As far as I understand Matlab (I'm very new to it), I am scaling the graph on my own. There is a function called draw1d that is in a protected file here: http://emlab.utep.edu/ee5390fdtd.htm
How should I fix this?
%FDTD1D
%Initailize
close all;
clc;
clear all;
% Units
meters = 1;
seconds = 1;
%Fundamental constants
c0 = 3e8 * meters/seconds;
e0 = 8.85e-12 * 1/meters;
u0 = 1.26e-6 * 1/meters;
%Figure Window
figure('Color', 'b');
%%%%%%%
%% FUNAMENTAL CONSTANTS
%%%%%%%%%
%Simple parameters
dz = 5 * meters;
Nz = 200;
dt = 1e-3 * seconds;
STEPS = 1000;
%%%%%%%
%% BUILD DEVICE ON GRID
%%%%%%%%%
%Grid Device - let it be air
ER = ones(1, Nz);
UR = ones(1, Nz);
%%%%%%%
%% Initialize FDTD Parameters
%%%%%%%%%
%Initialize Vectors
mEy = (c0*dt)./ER; %unique update coefficient for each place on the grid
mHx = (c0*dt)./UR; %unique update coefficent again
%%%%%%%
%% Initialize Fields
%%%%%%%%%
%Initialize Fields
Ey = zeros(1, Nz);
Hx = zeros(1, Nz);
%%%%%%%
%% IFDTD LOOP ANALYSIS
%%%%%%%%%
%Actually doing FDTD
for T = 1 : STEPS
%Update H from E
for nz = 1 : Nz - 1
Hx(nz) = Hx(nz) + mHx(nz)*(Ey(nz+1) - Ey(nz))/dz;
end
Hx(Nz) = Hx(Nz) + mHx(Nz)*(0 - Ey(Nz))/dz;
%Update E from H
Ey(1) = Ey(1) + mEy(1) * ( Hx(1) - 0 )/dz;
for nz = 2 : Nz
Ey(nz) = Ey(nz) + mEy(nz) * ( Hx(nz) - Hx(nz-1) )/dz;
end
%Show Status
if ~mod(T, 10)
draw1d(ER, Ey, Hx, dz);
xlim([dz Nz*dz]);
xlabel('z');
title(['Field at step ' num2str(T) ' of ' num2str(STEPS)]);
end
end

Jacobi method converging then diverging

I am working to solve Poisson's equation (in 2d axisymmetric cylindrical coordinates) using the Jacobi method. The L2 norm decreases from ~1E3 on the first iteration (I have a really bad guess) to ~0.2 very slowly. Then, the L2 norm begins to increase over many iterations.
My geometry is parallel plates with sharp points at r = 0 on both plates. (If that matters).
Is there some error in my code? Do I need to go to a different algorithm? (I have a not yet working DADI algorithm.)
Here is my Jacobi method algorithm. Then this just in wrapped in a while loop.
subroutine Jacobi(PoissonRHS, V, resid)
implicit none
real, dimension(0:,0:) :: PoissonRHS, V
REAL resid
integer :: i,j, lb, ub
real, dimension(0:size(V,1)-1, 0:size(V,2)-1) :: oldV
real :: dr = delta(1)
real :: dz = delta(2)
real :: dr2 = (delta(1))**(-2)
real :: dz2 = (delta(2))**(-2)
integer :: M = cells(1)
integer :: N = cells(2)
oldV = V
!Note: All of the equations are second order accurate
!If at r = 0 and in the computational domain
! This is the smoothness condition, dV(r=0)/dr = 0
V(0,:) = (4.0*oldV(1,:)-oldV(2,:))/3.0
!If at r = rMax and in the computational domain
! This is an approximation and should be fixed to improve accuracy, it should be
! lim r->inf V' = 0, while this is V'(r = R) = 0
V(M, 1:N-1) = 0.5 / (dr2 + dz2) * ( &
(2.0*dr2)*oldV(M-1,1:N-1) + &
dz2 * (oldV(M,2:N) + oldV(M,0:N-2)) &
- PoissonRHS(M,1:N-1))
do i = 1, M-1
lb = max(0, nint(lowerBoundary(i * dr) / dz)) + 1
ub = min(N, nint(upperBoundary(i * dr) / dz)) - 1
V(i,lb:ub) = 0.5 / (dr2 + dz2) * ( &
((1.0 - 0.5/dble(i))*dr2)*oldV(i-1,lb:ub) + &
((1.0 + 0.5/dble(i))*dr2)*oldV(i+1,lb:ub) + &
dz2 * (oldV(i,lb+1:ub+1) + oldV(i,lb-1:ub-1)) &
- PoissonRHS(i,lb:ub))
V(i, 0:lb-1) = V0
V(i, ub+1:N) = VL
enddo
!compare to old V values to check for convergence
resid = sqrt(sum((oldV-V)**2))
return
end subroutine Jacobi
Based on additional readings it seems like it was a precision problem. Because (for example), I had the expression
V(i,lb:ub) = 0.5 / (dr2 + dz2) * ( &
((1.0 - 0.5/dble(i))*dr2)*oldV(i-1,lb:ub) + &
((1.0 + 0.5/dble(i))*dr2)*oldV(i+1,lb:ub) + &
dz2 * (oldV(i,lb+1:ub+1) + oldV(i,lb-1:ub-1)) &
- PoissonRHS(i,lb:ub))
where dr2 and dz2 are very large. So by distributing these I got terms that were ~1 and the code converges (slowly, but that's a function of the mathematics).
So my new code is
subroutine Preconditioned_Jacobi(PoissonRHS, V, resid)
implicit none
real, dimension(0:,0:) :: PoissonRHS, V
REAL resid
integer :: i,j, lb, ub
real, dimension(0:size(V,1)-1, 0:size(V,2)-1) :: oldV
real :: dr = delta(1)
real :: dz = delta(2)
real :: dr2 = (delta(1))**(-2)
real :: dz2 = (delta(2))**(-2)
real :: b,c,d
integer :: M = cells(1)
integer :: N = cells(2)
b = 0.5*(dr**2)/((dr**2) + (dz**2))
c = 0.5*(dz**2)/((dr**2) + (dz**2))
d = -0.5 / (dr2 + dz2)
oldV = V
!Note: All of the equations are second order accurate
!If at r = 0 and in the computational domain
! This is the smoothness condition, dV(r=0)/dr = 0
V(0,:) = (4.0*oldV(1,:)-oldV(2,:))/3.0 !same as: oldV(0,:) - 2.0/3.0 * (1.5 * oldV(0,:) - 2.0 * oldV(1,:) + 0.5 * oldV(2,:) - 0)
!If at r = rMax and in the computational domain
! This is an approximation and should be fixed to improve accuracy, it should be
! lim r->inf V' = 0, while this is V'(r = R) = 0
V(M,1:N-1) = d*PoissonRHS(M,1:N-1) &
+ 2.0*c * oldV(M-1,1:N-1) &
+ b * ( oldV(M,0:N) + oldV(M,2:N) )
do i = 1, M-1
lb = max(0, nint(lowerBoundary(i * dr) / dz)) + 1
ub = min(N, nint(upperBoundary(i * dr) / dz)) - 1
V(i,lb:ub) = d*PoissonRHS(i,lb:ub) &
+ (c * (1.0-0.5/dble(i)) * oldV(i-1,lb:ub)) &
+ (c * (1.0+0.5/dble(i)) * oldV(i+1,lb:ub)) &
+ b * (oldV(i,lb-1:ub-1) + oldV(i,lb+1:ub+1))
V(i, 0:lb-1) = V0
V(i, ub+1:N) = VL
enddo
!compare to old V values to check for convergence
resid = sum(abs(oldV-V))
return
end subroutine Preconditioned_Jacobi

Trilateration and locating the point (x,y,z)

I want to find the coordinate of an unknown node which lie somewhere in the space which has its reference distance away from 3 or more nodes which all of them have known coordinate.
This problem is exactly like Trilateration as described here Trilateration.
However, I don't understand the part about "Preliminary and final computations" (refer to the wikipedia site). I don't get where I could find P1, P2 and P3 just so I can put to those equation?
Thanks
Trilateration is the process of finding the center of the area of intersection of three spheres. The center point and radius of each of the three spheres must be known.
Let's consider your three example centerpoints P1 [-1,1], P2 [1,1], and P3 [-1,-1]. The first requirement is that P1' be at the origin, so let us adjust the points accordingly by adding an offset vector V [1,-1] to all three:
P1' = P1 + V = [0, 0]
P2' = P2 + V = [2, 0]
P3' = P3 + V = [0,-2]
Note: Adjusted points are denoted by the ' (prime) annotation.
P2' must also lie on the x-axis. In this case it already does, so no adjustment is necessary.
We will assume the radius of each sphere to be 2.
Now we have 3 equations (given) and 3 unknowns (X, Y, Z of center-of-intersection point).
Solve for P4'x:
x = (r1^2 - r2^2 + d^2) / 2d //(d,0) are coords of P2'
x = (2^2 - 2^2 + 2^2) / 2*2
x = 1
Solve for P4'y:
y = (r1^2 - r3^2 + i^2 + j^2) / 2j - (i/j)x //(i,j) are coords of P3'
y = (2^2 - 2^2 + 0 + -2^2) / 2*-2 - 0
y = -1
Ignore z for 2D problems.
P4' = [1,-1]
Now we translate back to original coordinate space by subtracting the offset vector V:
P4 = P4' - V = [0,0]
The solution point, P4, lies at the origin as expected.
The second half of the article is describing a method of representing a set of points where P1 is not at the origin or P2 is not on the x-axis such that they fit those constraints. I prefer to think of it instead as a translation, but both methods will result in the same solution.
Edit: Rotating P2' to the x-axis
If P2' does not lie on the x-axis after translating P1 to the origin, we must perform a rotation on the view.
First, let's create some new vectors to use as an example:
P1 = [2,3]
P2 = [3,4]
P3 = [5,2]
Remember, we must first translate P1 to the origin. As always, the offset vector, V, is -P1. In this case, V = [-2,-3]
P1' = P1 + V = [2,3] + [-2,-3] = [0, 0]
P2' = P2 + V = [3,4] + [-2,-3] = [1, 1]
P3' = P3 + V = [5,2] + [-2,-3] = [3,-1]
To determine the angle of rotation, we must find the angle between P2' and [1,0] (the x-axis).
We can use the dot product equality:
A dot B = ||A|| ||B|| cos(theta)
When B is [1,0], this can be simplified: A dot B is always just the X component of A, and ||B|| (the magnitude of B) is always a multiplication by 1, and can therefore be ignored.
We now have Ax = ||A|| cos(theta), which we can rearrange to our final equation:
theta = acos(Ax / ||A||)
or in our case:
theta = acos(P2'x / ||P2'||)
We calculate the magnitude of P2' using ||A|| = sqrt(Ax + Ay + Az)
||P2'|| = sqrt(1 + 1 + 0) = sqrt(2)
Plugging that in we can solve for theta
theta = acos(1 / sqrt(2)) = 45 degrees
Now let's use the rotation matrix to rotate the scene by -45 degrees.
Since P2'y is positive, and the rotation matrix rotates counter-clockwise, we'll use a negative rotation to align P2 to the x-axis (if P2'y is negative, don't negate theta).
R(theta) = [cos(theta) -sin(theta)]
[sin(theta) cos(theta)]
R(-45) = [cos(-45) -sin(-45)]
[sin(-45) cos(-45)]
We'll use double prime notation, '', to denote vectors which have been both translated and rotated.
P1'' = [0,0] (no need to calculate this one)
P2'' = [1 cos(-45) - 1 sin(-45)] = [sqrt(2)] = [1.414]
[1 sin(-45) + 1 cos(-45)] = [0] = [0]
P3'' = [3 cos(-45) - (-1) sin(-45)] = [sqrt(2)] = [ 1.414]
[3 sin(-45) + (-1) cos(-45)] = [-2*sqrt(2)] = [-2.828]
Now you can use P1'', P2'', and P3'' to solve for P4''. Apply the reverse rotation to P4'' to get P4', then the reverse translation to get P4, your center point.
To undo the rotation, multiply P4'' by R(-theta), in this case R(45). To undo the translation, subtract the offset vector V, which is the same as adding P1 (assuming you used -P1 as your V originally).
This is the algorithm I use in a 3D printer firmware. It avoids rotating the coordinate system, but it may not be the best.
There are 2 solutions to the trilateration problem. To get the second one, replace "- sqrtf" by "+ sqrtf" in the quadratic equation solution.
Obviously you can use doubles instead of floats if you have enough processor power and memory.
// Primary parameters
float anchorA[3], anchorB[3], anchorC[3]; // XYZ coordinates of the anchors
// Derived parameters
float Da2, Db2, Dc2;
float Xab, Xbc, Xca;
float Yab, Ybc, Yca;
float Zab, Zbc, Zca;
float P, Q, R, P2, U, A;
...
inline float fsquare(float f) { return f * f; }
...
// Precompute the derived parameters - they don't change unless the anchor positions change.
Da2 = fsquare(anchorA[0]) + fsquare(anchorA[1]) + fsquare(anchorA[2]);
Db2 = fsquare(anchorB[0]) + fsquare(anchorB[1]) + fsquare(anchorB[2]);
Dc2 = fsquare(anchorC[0]) + fsquare(anchorC[1]) + fsquare(anchorC[2]);
Xab = anchorA[0] - anchorB[0];
Xbc = anchorB[0] - anchorC[0];
Xca = anchorC[0] - anchorA[0];
Yab = anchorA[1] - anchorB[1];
Ybc = anchorB[1] - anchorC[1];
Yca = anchorC[1] - anchorA[1];
Zab = anchorB[2] - anchorC[2];
Zbc = anchorB[2] - anchorC[2];
Zca = anchorC[2] - anchorA[2];
P = ( anchorB[0] * Yca
- anchorA[0] * anchorC[1]
+ anchorA[1] * anchorC[0]
- anchorB[1] * Xca
) * 2;
P2 = fsquare(P);
Q = ( anchorB[1] * Zca
- anchorA[1] * anchorC[2]
+ anchorA[2] * anchorC[1]
- anchorB[2] * Yca
) * 2;
R = - ( anchorB[0] * Zca
+ anchorA[0] * anchorC[2]
+ anchorA[2] * anchorC[0]
- anchorB[2] * Xca
) * 2;
U = (anchorA[2] * P2) + (anchorA[0] * Q * P) + (anchorA[1] * R * P);
A = (P2 + fsquare(Q) + fsquare(R)) * 2;
...
// Calculate Cartesian coordinates given the distances to the anchors (La, Lb and Lc)
// First calculate PQRST such that x = (Qz + S)/P, y = (Rz + T)/P.
// P, Q and R depend only on the anchor positions, so they are pre-computed
const float S = - Yab * (fsquare(Lc) - Dc2)
- Yca * (fsquare(Lb) - Db2)
- Ybc * (fsquare(La) - Da2);
const float T = - Xab * (fsquare(Lc) - Dc2)
+ Xca * (fsquare(Lb) - Db2)
+ Xbc * (fsquare(La) - Da2);
// Calculate quadratic equation coefficients
const float halfB = (S * Q) - (R * T) - U;
const float C = fsquare(S) + fsquare(T) + (anchorA[1] * T - anchorA[0] * S) * P * 2 + (Da2 - fsquare(La)) * P2;
// Solve the quadratic equation for z
float z = (- halfB - sqrtf(fsquare(halfB) - A * C))/A;
// Substitute back for X and Y
float x = (Q * z + S)/P;
float y = (R * z + T)/P;
Here are the Wikipedia calculations, presented in an OpenSCAD script, which I think helps to understand the problem in a visual wayand provides an easy way to check that the results are correct. Example output from the script
// Trilateration example
// from Wikipedia
//
// pA, pB and pC are the centres of the spheres
// If necessary the spheres must be translated
// and rotated so that:
// -- all z values are 0
// -- pA is at the origin
pA = [0,0,0];
// -- pB is on the x axis
pB = [10,0,0];
pC = [9,7,0];
// rA , rB and rC are the radii of the spheres
rA = 9;
rB = 5;
rC = 7;
if ( pA != [0,0,0]){
echo ("ERROR: pA must be at the origin");
assert(false);
}
if ( (pB[2] !=0 ) || pC[2] !=0){
echo("ERROR: all sphere centers must be in z = 0 plane");
assert(false);
}
if (pB[1] != 0){
echo("pB centre must be on the x axis");
assert(false);
}
// show the spheres
module spheres(){
translate (pA){
sphere(r= rA, $fn = rA * 10);
}
translate(pB){
sphere(r = rB, $fn = rB * 10);
}
translate(pC){
sphere (r = rC, $fn = rC * 10);
}
}
function unit_vector( v) = v / norm(v);
ex = unit_vector(pB - pA) ;
echo(ex = ex);
i = ex * ( pC - pA);
echo (i = i);
ey = unit_vector(pC - pA - i * ex);
echo (ey = ey);
d = norm(pB - pA);
echo (d = d);
j = ey * ( pC - pA);
echo (j = j);
x = (pow(rA,2) - pow(rB,2) + pow(d,2)) / (2 * d);
echo( x = x);
// size of the cube to subtract to show
// the intersection of the spheres
cube_size = [10,10,10];
if ( ((d - rA) >= rB) || ( rB >= ( d + rA)) ){
echo ("Error Y not solvable");
}else{
y = (( pow(rA,2) - pow(rC,2) + pow(i,2) + pow(j,2)) / (2 * j))
- ( i / j) * x;
echo(y = y);
zpow2 = pow(rA,2) - pow(x,2) - pow(y,2);
if ( zpow2 < 0){
echo ("z not solvable");
}else{
z = sqrt(zpow2);
echo (z = z);
// subtract a cube with one of its corners
// at the point where the sphers intersect
difference(){
spheres();
translate ([x,y - cube_size[1],z]){
cube(cube_size);
}
}
translate ([x,y - cube_size[1],z]){
%cube(cube_size);
}
}
}

Resources