I have to re-implement B-splines in MATLAB (interpolating natural B-spline, 3rd degree), but I have some issues making the B-spline natural (this means that S"(a) = S"(b) = 0 where S is my interpolating function in [a,b]). That's what I did so far implementing De Boor's algorithm:
function [ b ] = deBoore( p,i,x,y )
% p is the degree, i is the index of the bspline
n = length(x);
if p==0
b = zeros(1,n);
for j=1:n
if y(i)<=x(j) && x(j)<y(i+1)
b(j) = 1;
end
end
else
b = (((x-y(i))/(y(i+p)-y(i))).*deBoore(p-1,i,x,y)) ...
+ (((y(i+p+1)-x)/(y(i+p+1)-y(i+1))).*deBoore(p-1,i+1,x,y));
end
end
So, this should work just fine, but I have to build the interpolating polynomial to look something like this:
function [ sp ] = splineB( p, x, y, f )
m = length(y);
n = length(f);
sp = 0;
for i=(1):(m-p-1)
sp = sp + f(i)*deBoore(p,i,x,y);
end
end
It's almost working, but it's like the interpolation starts later than it is supposed to. What am I doing wrong and how do I fix it?
Related
I am simply trying to draw a cubic B-Spline using the matrix representation given in this paper: http://vision.ucsd.edu/~kbranson/research/bsplines/bsplines.pdf
Specifically, I am trying the to exactly replicate the formula in section 3 (using placement matrix G) of the PDF. But I am not sure where I am going wrong. It keeps producing straight lines. Can anyone point me out what is wrong with the following code (it should run on any version of matlab; its quite simple) ?
% main.m
clc; clear;
n_cpts = 5;
deg = 3;
cpts = randi(30, n_cpts, 2);
n_knots = n_cpts + deg + 1;
knots = 0:(n_knots-1);
ts = knots(deg):0.05:knots(end-deg);
curve = [];
for t = ts(1:end-1)
k = floor(t);
T = [1, t, t^2, t^3];
B = Bi(k);
G = Gi(k, n_cpts);
p = T * B * G * cpts;
curve = [curve; p];
end
scatter(curve(:,1), curve(:,2));
The helper functions
% Bi.m
% The 'B' matrix
function [B] = Bi(i)
B = [[ -(1./6.)*i^3, (1./6.)*(3*i^3 + 3*i^2 - 3*i + 1), -(1./2.)*(i^3)-(i^2)+2./3., (1./2.)*(i+1)^3 ];
[ +(1./2.)*i^2, -(1./2.)*(3*i-1)*(i+1), (1./2.)*(3*i^2 + 4*i), -(1./2.)*(i+1)^2 ];
[ (1./2.)*i, (1./2.)*(3*i+1), -(1./2.)*(3*i+2), (1./2.)*(i+1) ];
[ (1./6.), -(1./2.), (1./2.), -(1./6.) ]];
end
% Gi.m
% The 'G' matrix
function [G] = Gi(i, L)
G = zeros(4, L);
for m = 1:4
for n = 1:L
if n == i+m-3
G(m,n) = 1;
end
end
end
end
My output looks like this:
I am trying to simulate the rotation dynamics of a system. I am testing my code to verify that it's working using simulation, but I never recovered the parameters I pass to the model. In other words, I can't re-estimate the parameters I chose for the model.
I am using MATLAB for that and specifically ode45. Here is my code:
% Load the input-output data
[torque outputs] = DataLogs2();
u = torque;
% using the simulation data
Ixx = 1.00;
Iyy = 2.00;
Izz = 3.00;
x0 = [0; 0; 0];
Ts = .02;
t = 0:Ts:Ts * ( length(u) - 1 );
[ T, x ] = ode45( #(t,x) rotationDyn( t, x, u(1+floor(t/Ts),:), Ixx, Iyy, Izz), t, x0 );
w = x';
N = length(w);
q = 1; % a counter for the A and B matrices
% The Algorithm
for k=1:1:N
w_telda = [ 0 -w(3, k) w(2,k); ...
w(3,k) 0 -w(1,k); ...
-w(2,k) w(1,k) 0 ];
if k == N % to handle the problem of the last iteration
w_dash(:,k) = (-w(:,k))/Ts;
else
w_dash(:,k) = (w(:,k+1)-w(:,k))/Ts;
end
a = kron( w_dash(:,k)', eye(3) ) + kron( w(:,k)', w_telda );
A(q:q+2,:) = a; % a 3N*9 matrix
B(q:q+2,:) = u(k,:)'; % a 3N*1 matrix % u(:,k)
q = q + 3;
end
% Forcing J to be diagonal. This is the case when we consider our quadcopter as two thin uniform
% rods crossed at the origin with a point mass (motor) at the end of each.
A_new = [A(:, 1) A(:, 5) A(:, 9)];
vec_J_diag = A_new\B;
J_diag = diag([vec_J_diag(1), vec_J_diag(2), vec_J_diag(3)])
eigenvalues_J_diag = eig(J_diag)
error = norm(A_new*vec_J_diag - B)
where my dynamic model is defined as:
function [dw, y] = rotationDyn(t, w, tau, Ixx, Iyy, Izz, varargin)
% The output equation
y = [w(1); w(2); w(3)];
% State equation
% dw = (I^-1)*( tau - cross(w, I*w) );
dw = [Ixx^-1 * tau(1) - ((Izz-Iyy)/Ixx)*w(2)*w(3);
Iyy^-1 * tau(2) - ((Ixx-Izz)/Iyy)*w(1)*w(3);
Izz^-1 * tau(3) - ((Iyy-Ixx)/Izz)*w(1)*w(2)];
end
Practically, what this code should do, is to calculate the eigenvalues of the inertia matrix, J, i.e. to recover Ixx, Iyy, and Izz that I passed to the model at the very begining (1, 2 and 3), but all what I get is wrong results.
Is the problem with using ode45?
Well the problem wasn't in the ode45 instruction, the problem is that in system identification one can create an n-1 samples-signal from an n samples-signal, thus the loop has to end at N-1 in the above code.
I need to create a symbolic matrix in MATLAB. It can be done statically as
syms a11 a12 a21 a22;
A = [a11 a12; a21 a22];
or using compact syntax as
A = sym('A%d', [2 2]);
However i don't see how any of these syntax's should allow dynamic initialization. I would like to initialize each matrix element individually using two loops. One shot at a possible syntax (it's NOT valid MATLAB syntax) could be
syms a x1;
P = sym(zeros(n,n));
for l = 1:n
for m = 1:n
kl = l; km = m;
P(l,m)(x1) = a*sin(kl*x1)*sin(km*x1);
end
end
where I used the (invalid) syntax P(l,m)(x1) to specify that each element in P is a function of x1. Hence P(2) would be an (n,n) matrix with elements P(l,m) = a*sin(kl*2)*sin(km*2). I know it's possible to construct the P by building the matrix string (A=[...]) on run time and evaluating using eval. Something like
syms a x1;
command = [];
for i = 1:n
for j = 1:n
kl = i; km = j;
command = [command ' + a*sin(' num2str(kl) '*x1)*sin(' num2str(km) '*x1)'];
if(j < n) command = [command ',']; end
end
if(i < n) command = [command ';']; end
end
eval(['P(x1) = [' command '];'])
However, using eval is bad practice, so I see this solution only as a last resort. Is there any correct way to achieve my goal?
NB: I have written the elements P(l,m) = a*sin(kl*x1)*sin(km*x1) to simplify the code. The actual expression is P(l,m) = sin(kl*x1)*sin(km*x1)*epsilon + kl*km*cos(kl*x1)*cos(km*x1).*b + km*cos(km*x1)*sin(kl*x1)-kl*cos(kl*x1)*sin(km*x1)/(kl^2-km^2).
If you're just trying to avoid the for loops, you can use meshgrid, which hides them from you:
syms a x1
n = 3;
x = meshgrid(1:n)*x1; % multiplying by the symbolic x1 makes x symbolic
P = a*sin(x).*sin(x.');
which returns
P =
[ a*sin(x1)^2, a*sin(2*x1)*sin(x1), a*sin(3*x1)*sin(x1)]
[ a*sin(2*x1)*sin(x1), a*sin(2*x1)^2, a*sin(2*x1)*sin(3*x1)]
[ a*sin(3*x1)*sin(x1), a*sin(2*x1)*sin(3*x1), a*sin(3*x1)^2]
Given an amount of points in x and y I want to create splines that intersect all points and that have the same slopes in intersections.
My approach has been to establish a set of equations for intersection of points as well as dictating equal slopes at intersections and then use fsolve() to determine coefficients.
However, when plotting the found splines they do not have the same slopes at intersecting points though they do intersect the correct points given in x and y.
I have been trying to debug this script for most of two days now without any luck. Can someone point out why my splines are not getting the correct slopes? Or can it be that fsolve() quits before a satisfactory solution has been found?
Main file:
result = fsolve(#(K) eqns(x,y,K) , ones(1,(size(x,1)-1)*3) ); %Calls eqns() in eqns.m
A = result(1 : size(x,1)-1 );
B = result(size(x,1) : 2*size(x,1)-2 );
C = result(2*size(x,1)-1 : 3*size(x,1)-3 );
%Plot splines
splinePts = size(A,2)*100;
x_spline = [0 : x(end)/splinePts : x(end)];
fx = ones(splinePts,1);
for i = 1:size(A,2)
for j = 1:100
k = i*100-100 + j;
fx(k) = A(i) * x_spline(k)^2 + B(i) * x_spline(k) + C(i);
end
end
plot(fx);
eqns.m
function fcns=eqns(x,y,K)
A = K(1 : size(x,1)-1 ); %Coefficients for X^2
B = K(size(x,1) : 2*size(x,1)-2); %Coefficients for X
C = K(2*size(x,1)-1 : 3*size(x,1)-3); %Constants
%Equations for hitting all points
syms H;
temp = H; %Initiate variable for containing equations.
for i = 1:size(B,2)
temp(end+1) = eqn(x(i),y(i),A,B,C,i); %Calls eqn() in eqn.
temp(end+1) = eqn(x(i+1),y(i+1),A,B,C,i);
end
%Equations for slopes at spline intersections
syms X;
temp(end+1) = subs( diff(eqn(X,0,A,B,C,1),X) - 0 , 'X' , x(1) );
for i = 1:size(A,2)-1
temp(end+1) = subs( diff(eqn(X,1,A,B,C,i),X) - diff(eqn(X,1,A,B,C,i+1),X) , 'X' , x(i)+1 );
end
fcns = double( temp(2:end) );
end
eqn.m
function fcn=eqn(X,Y,A,B,C,i)
fcn = A(i)*X^2 + B(i)*X + C(i) - Y;
end
I'm trying to write a cubic spline interpolation program. I have written the program but, the graph is not coming out correctly. The spline uses natural boundary conditions(second dervative at start/end node are 0). The code is in Matlab and is shown below,
clear all
%Function to Interpolate
k = 10; %Number of Support Nodes-1
xs(1) = -1;
for j = 1:k
xs(j+1) = -1 +2*j/k; %Support Nodes(Equidistant)
end;
fs = 1./(25.*xs.^2+1); %Support Ordinates
x = [-0.99:2/(2*k):0.99]; %Places to Evaluate Function
fx = 1./(25.*x.^2+1); %Function Evaluated at x
%Cubic Spline Code(Coefficients to Calculate 2nd Derivatives)
f(1) = 2*(xs(3)-xs(1));
g(1) = xs(3)-xs(2);
r(1) = (6/(xs(3)-xs(2)))*(fs(3)-fs(2)) + (6/(xs(2)-xs(1)))*(fs(1)-fs(2));
e(1) = 0;
for i = 2:k-2
e(i) = xs(i+1)-xs(i);
f(i) = 2*(xs(i+2)-xs(i));
g(i) = xs(i+2)-xs(i+1);
r(i) = (6/(xs(i+2)-xs(i+1)))*(fs(i+2)-fs(i+1)) + ...
(6/(xs(i+1)-xs(i)))*(fs(i)-fs(i+1));
end
e(k-1) = xs(k)-xs(k-1);
f(k-1) = 2*(xs(k+1)-xs(k-1));
r(k-1) = (6/(xs(k+1)-xs(k)))*(fs(k+1)-fs(k)) + ...
(6/(xs(k)-xs(k-1)))*(fs(k-1)-fs(k));
%Tridiagonal System
i = 1;
A = zeros(k-1,k-1);
while i < size(A)+1;
A(i,i) = f(i);
if i < size(A);
A(i,i+1) = g(i);
A(i+1,i) = e(i);
end
i = i+1;
end
for i = 2:k-1 %Decomposition
e(i) = e(i)/f(i-1);
f(i) = f(i)-e(i)*g(i-1);
end
for i = 2:k-1 %Forward Substitution
r(i) = r(i)-e(i)*r(i-1);
end
xn(k-1)= r(k-1)/f(k-1);
for i = k-2:-1:1 %Back Substitution
xn(i) = (r(i)-g(i)*xn(i+1))/f(i);
end
%Interpolation
if (max(xs) <= max(x))
error('Outside Range');
end
if (min(xs) >= min(x))
error('Outside Range');
end
P = zeros(size(length(x),length(x)));
i = 1;
for Counter = 1:length(x)
for j = 1:k-1
a(j) = x(Counter)- xs(j);
end
i = find(a == min(a(a>=0)));
if i == 1
c1 = 0;
c2 = xn(1)/6/(xs(2)-xs(1));
c3 = fs(1)/(xs(2)-xs(1));
c4 = fs(2)/(xs(2)-xs(1))-xn(1)*(xs(2)-xs(1))/6;
t1 = c1*(xs(2)-x(Counter))^3;
t2 = c2*(x(Counter)-xs(1))^3;
t3 = c3*(xs(2)-x(Counter));
t4 = c4*(x(Counter)-xs(1));
P(Counter) = t1 +t2 +t3 +t4;
else
if i < k-1
c1 = xn(i-1+1)/6/(xs(i+1)-xs(i-1+1));
c2 = xn(i+1)/6/(xs(i+1)-xs(i-1+1));
c3 = fs(i-1+1)/(xs(i+1)-xs(i-1+1))-xn(i-1+1)*(xs(i+1)-xs(i-1+1))/6;
c4 = fs(i+1)/(xs(i+1)-xs(i-1+1))-xn(i+1)*(xs(i+1)-xs(i-1+1))/6;
t1 = c1*(xs(i+1)-x(Counter))^3;
t2 = c2*(x(Counter)-xs(i-1+1))^3;
t3 = c3*(xs(i+1)-x(Counter));
t4 = c4*(x(Counter)-xs(i-1+1));
P(Counter) = t1 +t2 +t3 +t4;
else
c1 = xn(i-1+1)/6/(xs(i+1)-xs(i-1+1));
c2 = 0;
c3 = fs(i-1+1)/(xs(i+1)-xs(i-1+1))-xn(i-1+1)*(xs(i+1)-xs(i-1+1))/6;
c4 = fs(i+1)/(xs(i+1)-xs(i-1+1));
t1 = c1*(xs(i+1)-x(Counter))^3;
t2 = c2*(x(Counter)-xs(i-1+1))^3;
t3 = c3*(xs(i+1)-x(Counter));
t4 = c4*(x(Counter)-xs(i-1+1));
P(Counter) = t1 +t2 +t3 +t4;
end
end
end
P = P';
P(length(x)) = NaN;
plot(x,P,x,fx)
When I run the code, the interpolation function is not symmetric and, it doesn't converge correctly. Can anyone offer any suggestions about problems in my code? Thanks.
I wrote a cubic spline package in Mathematica a long time ago. Here is my translation of that package into Matlab. Note I haven't looked at cubic splines in about 7 years, so I'm basing this off my own documentation. You should check everything I say.
The basic problem is we are given n data points (x(1), y(1)) , ... , (x(n), y(n)) and we wish to calculate a piecewise cubic interpolant. The interpolant is defined as
S(x) = { Sk(x) when x(k) <= x <= x(k+1)
{ 0 otherwise
Here Sk(x) is a cubic polynomial of the form
Sk(x) = sk0 + sk1*(x-x(k)) + sk2*(x-x(k))^2 + sk3*(x-x(k))^3
The properties of the spline are:
The spline pass through the data point Sk(x(k)) = y(k)
The spline is continuous at the end-points and thus continuous everywhere in the interpolation interval Sk(x(k+1)) = Sk+1(x(k+1))
The spline has continuous first derivative Sk'(x(k+1)) = Sk+1'(x(k+1))
The spline has continuous second derivative Sk''(x(k+1)) = Sk+1''(x(k+1))
To construct a cubic spline from a set of data point we need to solve for the coefficients
sk0, sk1, sk2 and sk3 for each of the n-1 cubic polynomials. That is a total of 4*(n-1) = 4*n - 4 unknowns. Property 1 supplies n constraints, and properties 2,3,4 each supply an additional n-2 constraints. Thus we have n + 3*(n-2) = 4*n - 6 constraints and 4*n - 4 unknowns. This leaves two degrees of freedom. We fix these degrees of freedom by setting the second derivative equal to zero at the start and end nodes.
Let m(k) = Sk''(x(k)) , h(k) = x(k+1) - x(k) and d(k) = (y(k+1) - y(k))/h(k). The following
three-term recurrence relation holds
h(k-1)*m(k-1) + 2*(h(k-1) + h(k))*m(k) + h(k)*m(k+1) = 6*(d(k) - d(k-1))
The m(k) are unknowns we wish to solve for. The h(k) and d(k) are defined by the input data.
This three-term recurrence relation defines a tridiagonal linear system. Once the m(k) are determined the coefficients for Sk are given by
sk0 = y(k)
sk1 = d(k) - h(k)*(2*m(k) + m(k-1))/6
sk2 = m(k)/2
sk3 = m(k+1) - m(k)/(6*h(k))
Okay that is all the math you need to know to completely define the algorithm to compute a cubic spline. Here it is in Matlab:
function [s0,s1,s2,s3]=cubic_spline(x,y)
if any(size(x) ~= size(y)) || size(x,2) ~= 1
error('inputs x and y must be column vectors of equal length');
end
n = length(x)
h = x(2:n) - x(1:n-1);
d = (y(2:n) - y(1:n-1))./h;
lower = h(1:end-1);
main = 2*(h(1:end-1) + h(2:end));
upper = h(2:end);
T = spdiags([lower main upper], [-1 0 1], n-2, n-2);
rhs = 6*(d(2:end)-d(1:end-1));
m = T\rhs;
% Use natural boundary conditions where second derivative
% is zero at the endpoints
m = [ 0; m; 0];
s0 = y;
s1 = d - h.*(2*m(1:end-1) + m(2:end))/6;
s2 = m/2;
s3 =(m(2:end)-m(1:end-1))./(6*h);
Here is some code to plot a cubic spline:
function plot_cubic_spline(x,s0,s1,s2,s3)
n = length(x);
inner_points = 20;
for i=1:n-1
xx = linspace(x(i),x(i+1),inner_points);
xi = repmat(x(i),1,inner_points);
yy = s0(i) + s1(i)*(xx-xi) + ...
s2(i)*(xx-xi).^2 + s3(i)*(xx - xi).^3;
plot(xx,yy,'b')
plot(x(i),0,'r');
end
Here is a function that constructs a cubic spline and plots in on the famous Runge function:
function cubic_driver(num_points)
runge = #(x) 1./(1+ 25*x.^2);
x = linspace(-1,1,num_points);
y = runge(x);
[s0,s1,s2,s3] = cubic_spline(x',y');
plot_points = 1000;
xx = linspace(-1,1,plot_points);
yy = runge(xx);
plot(xx,yy,'g');
hold on;
plot_cubic_spline(x,s0,s1,s2,s3);
You can see it in action by running the following at the Matlab prompt
>> cubic_driver(5)
>> clf
>> cubic_driver(10)
>> clf
>> cubic_driver(20)
By the time you have twenty nodes your interpolant is visually indistinguishable from the Runge function.
Some comments on the Matlab code: I don't use any for or while loops. I am able to vectorize all operations. I quickly form the sparse tridiagonal matrix with spdiags. I solve it using the backslash operator. I counting on Tim Davis's UMFPACK to handle the decomposition and forward and backward solves.
Hope that helps. The code is available as a gist on github https://gist.github.com/1269709
There was a bug in spline function, generated (n-2) by (n-2) should be symmetric:
lower = h(2:end);
main = 2*(h(1:end-1) + h(2:end));
upper = h(1:end-1);
http://www.mpi-hd.mpg.de/astrophysik/HEA/internal/Numerical_Recipes/f3-3.pdf