I have written a function in matlab that applies the newton method to a given system of equations and its jacobian. The x vector that approaches the intersection of the two functions given in the system of equations however is stored like this:
2.06733483062522 -0.615946742374531
2.25465524043858 -0.428626332561163
1.13788162420748 -1.54539994879227
1.32520203402085 -1.35807953897890
0.802454971005455 -1.88082660199429
0.989775380818824 -1.69350619218092
0.743382354827609 -1.93989921817214
0.930702764640978 -1.75257880835877
0.741429028102197 -1.94185254489755
0.928749437915565 -1.7545321350841
here
x1 = [2.06733~ ; 2.25465~]
and thus they are stored vertically but i want them horizontally underneath each other. Any help is appreciated
I've tried to take the values out in a for loop using even and odd entries but for a system of equations of higher order this wouldnt work.
% INPUT
% f root function (may be vector valued )
% df derivative of f (or function returning Jacobian matrix )
% x0 initial guess
% tol desired tolerance
% maxIt maximum number of iterations
%
% OUTPUT
% x approximate solution
% success true means converged according to error estimator
% errEst error estimate per iteration
% xHist ( optional ) array with intermediate solutions
function [x, success , errEst, xHist ] = newton (f, df, x0 , tol , maxIt )
errEst = []; xHist = [];
for k = 1:1:size(f)
iter = 0; err = inf; x = x0; success = false;
while err > 0 && iter < maxIt
Fun = f{k}(x);
Jac = df{k}(x);
delta = -Jac\Fun;
err = norm(delta);
x = x + delta;
errEst = [errEst; err];
xHist = [xHist; x];
iter = iter + 1;
end
end
if err < tol
success = true;
end
end
it is called like this:
x0 = 1/sqrt(2) * [2;1]; grad = [sqrt(2)/2;sqrt(2)]; n = grad/norm(grad); t = [n(2),-n(1)]; p = 0.5;
x0 = x0 + 2 * (n+t); %x0 = x0 - 2 * (n+t);
f = {#(x)(x(1)/2)^2 + x(2)^2 - 1, #(x)n .* (x - x0) - p - 0.5 .* (t .* (x - x0)) .^ 2};
df = {#(x)x(1)/2 + 2 * x(2), #(x)n - t .* (t .*(x - x0))};
[x, success , errEst , xHist ] = newton(f, df, x0 , 10^(-12) , 20);
Related
I've tried implementing Jacobi method for compressed sparse row format. But i couldnt obtain the output correctly. Below is the coding i tried. I'm trying with a 4 by 4 sparse matrix which is a tridiagonal matrix stored in compressed form before implementing Jacobi iterative method. Please help.
clear all;
close all;
clc;
H=4;
a=2;
b=-1;
c=-1;
A = diag(a*ones(1,H)) + diag(b*ones(1,H-1),1) + diag(c*ones(1,H-1),-1);%Matrix A
n = size(A,1); % no of rows
m = size(A,2); % no of columns
V = [];
C = [];
R = [];
counter=1;
R= [counter];
for i=1:n
for j=1:m
if (A(i,j) ~= 0)
V = [V A(i,j)];
C = [C j];
counter=counter+1;
end
R(i+1)=counter;
end
end
b = [9,18,24,3];
x_new = [1 ; 1 ; 1 ; 1];
eps = 1e-5; % 1 x 10^(-10).
error = 1000; % use any large value greater than eps to make sure that the loop can work
counter2=1;
while (error > eps)
x_old = x_new;
for i=1:length(R)-1 %modified
t = 0;
for j=R(i):R(i+1)-1 %modified
if (C(j)~=i) %not equal
t = t + x_old(C(j))*A(i,C(j)); %modified
end
end
x_new(i,1) = (b(i) - t)/A(i,C(j)); % is a row vector
end
error = norm(x_new-x_old);
counter2=counter2+1;
end
x_new % print x
Expected output is
[28.1987 47.3978 48.5979 25.7986]
this is the coding i tried and the expected output is above. Thank you for your time and consideration.
Suppose that X, Y are the matrices of coordinates inside the given intervals
xc = 0, yc = 0
xl = linspace(xc - 10, xc + 10, 2);
yl = linspace(yc - 10, yc + 10, 2);
[X,Y] = meshgrid(xl,yl);
and fun is a handle to some function test(v)
fun = #(v)test(v);
How to combine both matrices X, Y so that they represent components x,y of the vector v
res = arrayfun(fun, [X,Y]); //First processed X and then Y
Unfortunately, this solution does not work....
There is another way when the function is modified so that two parameters x, y are passed
fun = #(x, y)test(x, y);
res = arrayfun(fun, X, Y); //This works well
However, I would like to preserve an intertace of the function if any solution exists.
Thanks for your help.
Redefine fun as fun = #(x, y)test([x,y]);
No need to modify function test()
xc = 0;
yc = 0;
xl = linspace(xc - 10, xc + 10, 2);
yl = linspace(yc - 10, yc + 10, 2);
[X,Y] = meshgrid(xl,yl);
% Given function test
test =#(v)v(1) + v(2);
% pass x, y as a vector
fun = #(x, y)test([x,y]);
res = arrayfun(fun, X, Y);
% X =
-10 10
-10 10
% Y =
-10 -10
10 10
% fun(x, y) = x + y
% res =
-20 0
0 20
From Matlab doc:
B = arrayfun(func,A) applies the function func to the elements of A, one element at a time
B = arrayfun(func,A1,...,An) applies func to the elements of the arrays A1,...,An, so that B(i) = func(A1(i),...,An(i))
So you are using arrayfun in the wrong way.
Use a for loop or two nested loops instead.
for i=1:size(X,1)
for j=1:size(X,2)
res(i,j)=fun([X(i,j),Y(i,j)])
end
end
What are you trying to do?
Also, in Matlab, you should use % instead of // for commenting
These are some related questions:
arrayfun when each row of the array is an input
Passing a vector as multiple inputs to a function
Hopefully somebody can point out why this isnt working or where i may be going wrong. Im producing a sine wave by way of for loops in c. The ultimate aim is to produce a .ppm file displaying this. Im working on a 1 to 1 pixel ratio. My box is 128H*256W. The sine wave is displaying but due to the answer being produced in rads the result is a very small two pixel high "wave". I assume this is due to the rad values being between 1 and -1. This is my code. I tried just simply timesing by a greater number to increase the size of the y values in the hopes it would plot correctly but this does little or worse causes the applicattion to stop running. Any ideas very welcome.
for (x = 0; x < H; x++)
{
y =(int) H/2+ sin(x*(2*PI));
y = y * 50;
image[y][x][1] = 0;
image[y][x][2] = 255;
image[y][x][3] = 0;
}
EDIT: This is what is being produced in the .ppm file when opened via infraview. Also im #defining PI 3.141592653589793. Again is this possibly an area of issue.
first sine wave .ppm
I conject that y is an int.
Your sin value will be truncated to an integer; 0 for most cases, but very occasionally -1 or +1.
The fix is simple: use a floating point for y, and cast once you want to use it as an array index.
As y is commented to be an int and H appears to be an int constant, perform calculations as double first, then convert to int.
Use round to avoid truncations effect of simply casting a double to int.
y = (int) round(50*(sin(x*(2*PI)) + H/2.0));
Original code also scaled H/2 by 50. I think code may only want to scale the sin() and not the offset.
#define XOffset 0
#define YOffset (H/2.0)
#define XScale (2*PI)
#define YScale 50
y = (int) round(YScale*sin(x*XScale + XOffset) + YOffset);
Defensive programming tip: since y is calculated, insure it is in the valid index range before using it as an index.
// Assuming image` is a fixed sized array
#define Y_MAX (sizeof image/sizeof image[0] - 1)
if (y >= 0 && y <= Y_MAX) {
image[y][x][1] = 0;
image[y][x][2] = 255;
image[y][x][3] = 0;
}
y = y * 50, where y = H/2 (+ or - 1) gives you y around 25*H, which is out of bounds.
A closer approximation is this:
y = (int) ( H/2 + H/2 * sin(x*2*PI) )
which gives the extremes H/2 - H/2 = 0 and H/2 + H/2 = H, which is one too high. So, we scale not by H/2 but by (H-1)/2:
y = (int) ( H/2 + (H-1)/2 * sin(x*2*PI) )
which gives us an y-range 0 to H-1.
To have a bit more control over the period of the sine wave, let's write it like this:
sin( x/W * 2*PI )
Here, we divide x by W so that x/W itself will range from 0 to 1.
It is then scaled by 2*PI to produce a range from 0 to 2π. This will plot one period of the sine wave across the entire width. If we introduce a frequency factor f:
sin( f * x/W * 2*PI )
we can now say how many periods to draw, even fractions. For f=1 it will draw one period, f=2 two periods, and f=1 half a period.
Here's a small JS demo showing three values for f: 0.5 is red, 1 is green and 2 is white:
var c = document.getElementById('c'),
W = c.width,
H = c.height,
ctx = c.getContext('2d'),
image = ctx.getImageData(0,0,W,H);
for ( var i = 0; i < image.data.length; i +=4) {
image.data[i+0]=0;
image.data[i+1]=0;
image.data[i+2]=0;
image.data[i+3]=255;
}
function render(image,colidx,f) {
for ( var x = 0; x < W; x++ )
{
var y = H/2 - Math.round( H/2 * Math.sin(f*x/W*Math.PI*2) );
if ( y >=0 && y < H ) {
if ( colidx & 1 ) image.data[ 4*( W*y + x ) + 0] = 255;
if ( colidx & 2 ) image.data[ 4*( W*y + x ) + 1] = 255;
if ( colidx & 4 ) image.data[ 4*( W*y + x ) + 2] = 255;
}
}
}
render(image,1,0.5);
render(image,2,1);
render(image,7,2);
ctx.putImageData(image, 0,0);
canvas{ border: 1px solid red;}
<canvas id='c' width='256' height='128'></canvas>
The code then becomes:
float f = 1;
for (x = 0; x < W; x++)
{
y = (int) ( (H-1)/2 + (H-1)/2 * sin(f * x/W * 2*PI) );
image[y][x][0] = 0;
image[y][x][1] = 255;
image[y][x][2] = 0;
}
An analytical solution for cubic bezier length
seems not to exist, but it does not mean that
coding a cheap solution does not exist. By cheap I mean something like in the range of 50-100 ns (or less).
Does someone know anything like that? Maybe in two categories:
1) less error like 1% but more slow code.
2) more error like 20% but faster?
I scanned through google a bit but it doesn't
find anything which looks like a nice solution. Only something like divide on N line segments
and sum the N sqrt - too slow for more precision,
and probably too inaccurate for 2 or 3 segments.
Is there anything better?
Another option is to estimate the arc length as the average between the chord and the control net. In practice:
Bezier bezier = Bezier (p0, p1, p2, p3);
chord = (p3-p0).Length;
cont_net = (p0 - p1).Length + (p2 - p1).Length + (p3 - p2).Length;
app_arc_length = (cont_net + chord) / 2;
You can then recursively split your spline segment into two segments and calculate the arc length up to convergence. I tested myself and it actually converges pretty fast. I got the idea from this forum.
Simplest algorithm: flatten the curve and tally euclidean distance. As long as you want an approximate arc length, this solution is fast and cheap. Given your curve's coordinate LUT—you're talking about speed, so I'm assuming you use those, and don't constantly recompute the coordinates—it's a simple for loop with a tally. In generic code, with a dist function that computes the euclidean distance between two points:
var arclength = 0,
last=LUT.length-1,
i;
for (i=0; i<last; i++) {
arclength += dist(LUT[i], LUT[i+1]);
}
Done. arclength is now the approximate arc length based on the maximum number of segments you can form in the curve based on your LUT. Need things faster with a larger potential error? Control the segment count.
var arclength = 0,
segCount = ...,
last=LUT.length-2,
step = last/segCount,
s, i;
for (s=0; s<=segCount; s++) {
i = (s*step/last)|0;
arclength += dist(LUT[i], LUT[i+1]);
}
This is pretty much the simplest possible algorithm that still generates values that come even close to the true arc length. For anything better, you're going to have to use more expensive numerical approaches (like the Legendre-Gauss quadrature technique).
If you want to know why, hit up the arc length section of "A Primer on Bézier Curves".
in my case a fast and valid approach is this. (Rewritten in c# for Unity3d)
public static float BezierSingleLength(Vector3[] points){
var p0 = points[0] - points[1];
var p1 = points[2] - points[1];
var p2 = new Vector3();
var p3 = points[3]-points[2];
var l0 = p0.magnitude;
var l1 = p1.magnitude;
var l3 = p3.magnitude;
if(l0 > 0) p0 /= l0;
if(l1 > 0) p1 /= l1;
if(l3 > 0) p3 /= l3;
p2 = -p1;
var a = Mathf.Abs(Vector3.Dot(p0,p1)) + Mathf.Abs(Vector3.Dot(p2,p3));
if(a > 1.98f || l0 + l1 + l3 < (4 - a)*8) return l0+l1+l3;
var bl = new Vector3[4];
var br = new Vector3[4];
bl[0] = points[0];
bl[1] = (points[0]+points[1]) * 0.5f;
var mid = (points[1]+points[2]) * 0.5f;
bl[2] = (bl[1]+mid) * 0.5f;
br[3] = points[3];
br[2] = (points[2]+points[3]) * 0.5f;
br[1] = (br[2]+mid) * 0.5f;
br[0] = (br[1]+bl[2]) * 0.5f;
bl[3] = br[0];
return BezierSingleLength(bl) + BezierSingleLength(br);
}
I worked out the closed form expression of length for a 3 point Bezier (below). I've not attempted to work out a closed form for 4+ points. This would most likely be difficult or complicated to represent and handle. However, a numerical approximation technique such as a Runge-Kutta integration algorithm (see my Q&A here for details) would work quite well by integrating using the arc length formula.
Here is some Java code for the arc length of a 3 point Bezier, with points a,b, and c.
v.x = 2*(b.x - a.x);
v.y = 2*(b.y - a.y);
w.x = c.x - 2*b.x + a.x;
w.y = c.y - 2*b.y + a.y;
uu = 4*(w.x*w.x + w.y*w.y);
if(uu < 0.00001)
{
return (float) Math.sqrt((c.x - a.x)*(c.x - a.x) + (c.y - a.y)*(c.y - a.y));
}
vv = 4*(v.x*w.x + v.y*w.y);
ww = v.x*v.x + v.y*v.y;
t1 = (float) (2*Math.sqrt(uu*(uu + vv + ww)));
t2 = 2*uu+vv;
t3 = vv*vv - 4*uu*ww;
t4 = (float) (2*Math.sqrt(uu*ww));
return (float) ((t1*t2 - t3*Math.log(t2+t1) -(vv*t4 - t3*Math.log(vv+t4))) / (8*Math.pow(uu, 1.5)));
public float FastArcLength()
{
float arcLength = 0.0f;
ArcLengthUtil(cp0.position, cp1.position, cp2.position, cp3.position, 5, ref arcLength);
return arcLength;
}
private void ArcLengthUtil(Vector3 A, Vector3 B, Vector3 C, Vector3 D, uint subdiv, ref float L)
{
if (subdiv > 0)
{
Vector3 a = A + (B - A) * 0.5f;
Vector3 b = B + (C - B) * 0.5f;
Vector3 c = C + (D - C) * 0.5f;
Vector3 d = a + (b - a) * 0.5f;
Vector3 e = b + (c - b) * 0.5f;
Vector3 f = d + (e - d) * 0.5f;
// left branch
ArcLengthUtil(A, a, d, f, subdiv - 1, ref L);
// right branch
ArcLengthUtil(f, e, c, D, subdiv - 1, ref L);
}
else
{
float controlNetLength = (B-A).magnitude + (C - B).magnitude + (D - C).magnitude;
float chordLength = (D - A).magnitude;
L += (chordLength + controlNetLength) / 2.0f;
}
}
first of first you should Understand the algorithm use in Bezier,
When i was coding a program by c# Which was full of graphic material I used beziers and many time I had to find a point cordinate in bezier , whic it seem imposisble in the first look. so the thing i do was to write Cubic bezier function in my costume math class which was in my project. so I will share the code with you first.
//--------------- My Costum Power Method ------------------\\
public static float FloatPowerX(float number, int power)
{
float temp = number;
for (int i = 0; i < power - 1; i++)
{
temp *= number;
}
return temp;
}
//--------------- Bezier Drawer Code Bellow ------------------\\
public static void CubicBezierDrawer(Graphics graphics, Pen pen, float[] startPointPixel, float[] firstControlPointPixel
, float[] secondControlPointPixel, float[] endPointPixel)
{
float[] px = new float[1111], py = new float[1111];
float[] x = new float[4] { startPointPixel[0], firstControlPointPixel[0], secondControlPointPixel[0], endPointPixel[0] };
float[] y = new float[4] { startPointPixel[1], firstControlPointPixel[1], secondControlPointPixel[1], endPointPixel[1] };
int i = 0;
for (float t = 0; t <= 1F; t += 0.001F)
{
px[i] = FloatPowerX((1F - t), 3) * x[0] + 3 * t * FloatPowerX((1F - t), 2) * x[1] + 3 * FloatPowerX(t, 2) * (1F - t) * x[2] + FloatPowerX(t, 3) * x[3];
py[i] = FloatPowerX((1F - t), 3) * y[0] + 3 * t * FloatPowerX((1F - t), 2) * y[1] + 3 * FloatPowerX(t, 2) * (1F - t) * y[2] + FloatPowerX(t, 3) * y[3];
graphics.DrawLine(pen, px[i - 1], py[i - 1], px[i], py[i]);
i++;
}
}
as you see above, this is the way a bezier Function work and it draw the same Bezier as Microsoft Bezier Function do( I've test it). you can make it even more accurate by incrementing array size and counter size or draw elipse instead of line& ... . All of them depend on you need and level of accuracy you need and ... .
Returning to main goal ,the Question is how to calc the lenght???
well The answer is we Have tons of point and each of them has an x coorinat and y coordinate which remember us a triangle shape & especially A RightTriabgle Shape. so if we have point p1 & p2 , we can calculate the distance of them as a RightTriangle Chord. as we remeber from our math class in school, in ABC Triangle of type RightTriangle, chord Lenght is -> Sqrt(Angle's FrontCostalLenght ^ 2 + Angle's SideCostalLeghth ^ 2);
and there is this relation betwen all points we calc the lenght betwen current point and the last point before current point(exmp p[i - 1] & p[i]) and store sum of them all in a variable. lets show it in code bellow
//--------------- My Costum Power Method ------------------\\
public static float FloatPower2(float number)
{
return number * number;
}
//--------------- My Bezier Lenght Calculator Method ------------------\\
public static float CubicBezierLenghtCalculator(float[] startPointPixel
, float[] firstControlPointPixel, float[] secondControlPointPixel, float[] endPointPixel)
{
float[] tmp = new float[2];
float lenght = 0;
float[] px = new float[1111], py = new float[1111];
float[] x = new float[4] { startPointPixel[0], firstControlPointPixel[0]
, secondControlPointPixel[0], endPointPixel[0] };
float[] y = new float[4] { startPointPixel[1], firstControlPointPixel[1]
, secondControlPointPixel[1], endPointPixel[1] };
int i = 0;
for (float t = 0; t <= 1.0; t += 0.001F)
{
px[i] = FloatPowerX((1.0F - t), 3) * x[0] + 3 * t * FloatPowerX((1.0F - t), 2) * x[1] + 3F * FloatPowerX(t, 2) * (1.0F - t) * x[2] + FloatPowerX(t, 3) * x[3];
py[i] = FloatPowerX((1.0F - t), 3) * y[0] + 3 * t * FloatPowerX((1.0F - t), 2) * y[1] + 3F * FloatPowerX(t, 2) * (1.0F - t) * y[2] + FloatPowerX(t, 3) * y[3];
if (i > 0)
{
tmp[0] = Math.Abs(px[i - 1] - px[i]);// calculating costal lenght
tmp[1] = Math.Abs(py[i - 1] - py[i]);// calculating costal lenght
lenght += (float)Math.Sqrt(FloatPower2(tmp[0]) + FloatPower2(tmp[1]));// calculating the lenght of current RightTriangle Chord & add it each time to variable
}
i++;
}
return lenght;
}
if you wish to have faster calculation just need to reduce px & py array lenght and loob count.
We also can decrease memory need by reducing px and py to array lenght to 1 or make a simple double variable but becuase of Conditional situation Happend which Increase Our Big O I didn't do that.
Hope it helped you so much. if have another question just ask.
With Best regards, Heydar - Islamic Republic of Iran.
I'm trying to implement a simple script performing a PI control for a cruise control application, but I'm founding some problems with the integral part. Here is my code:
function [] = PI_cruisecontrol()
clc; close all;
t0 = 0; tfinal = 50; dt = 0.001; % time parameters
r = 10; % reference of 10 m/s
m = 1000; % mass
b = 50; % friction coeff. (depends on v)
yp = zeros(tfinal/dt,1); t = yp; % initialize speed and time array
Ki = 40; % integrarl constant
Kp = 800; % proportional constant
int = 0; % itinialize int error
% CONTROL LOOP (Forward-Euler integrator is used to solve the ODE)
for i=t0+2:tfinal/dt
err = r-yp(i-1); % update error
int = int+err; % integral term
u = (Kp*err)+(Ki*int*dt); % action of control
yp(i) = yp(i-1)+((-b*yp(i)/m) + (u/m))*dt; % solve ode for speed
t(i) = t(i)+dt*i; % log the time
end
% Results
figure(1)
plot(t,yp)
title ('Step Response')
xlabel('Time (seconds)')
ylabel('Amplitud')
axis([0 20 0 12])
hold on
reference = ones(tfinal/dt,1)*10;
plot(t,reference,':')
end
And this is how it should be, using predefinided matlab functions:
function [] = PI_cruisecontrol2()
m = 1000;
b = 50;
r = 10;
s = tf('s');
P_cruise = 1/(m*s + b);
Kp = 800;
Ki = 40;
C = pid(Kp,Ki);
T = feedback(C*P_cruise,1);
t = 0:0.1:20;
step(r*T,t)
axis([0 20 0 12])
end
What am I doing wrong in my code?
Thanks!
I managed to fix the problem, working with float variables instead of arrays. Moreover, I added the derivative term (although for this first order problem was not necessary)
Here I left the code:
function [] = aFortran_PI()
clc; close all;
r = 10; % reference of 10 m/s
m = 1000; % mass
b = 50; % friction coeff. (depends on v)
yp = 0; % init response
Kp = 800; % proportional constant
Ki = 40; % proportional constant
Kd = 0; % derivative term is not necessary in this problem
previous_error = 0;
integral = 0;
dt = 0.001;
% CONTROL LOOP
for i=1:20000
error = r-yp; % update error
integral = integral + error*dt; % integral term
derivative = (error-previous_error)/dt; % derivative term
u = Kp*error+Ki*integral+Kd*derivative; % action of control
yp = yp+(-b*yp/m + u/m)*dt % solve ode for velocity
end
end