Solving matrix Riccati differential equation in Matlab with terminal boundary condition - matlab

In the optimal control tracking problem, there is a Riccati equation of the gain matrix K(t) which is:
\dot{K}(t) = -K(t) A - A^{T} K(t) - Q + K(t) B R^{-1} B^{T} K(t)
At the final time of Tf, the terminal boundary condition K(Tf) is given.
Edit: After consideration, I think the question is how to numerically backward integrate the gain matrix with the given terminal boundary condition and save the results in a lookup table to obtain the solution over the interval [t0,Tf] for further computations in Simulink ?

The numerical solution to this equation is found in the book Optimal Control Systems
For example, the following is an excerpt of the technique:
E=B*inv(R)*B'; % the matrix E = BR^{-1}B'
%
% solve matrix difference Riccati equation backwards
% starting from kf to kO
% use the form P(k) = A'P(k+1)[I + EP(k+1)]^{-1}A + Q
% first fix the final condition P(k_f) = F
Pkplus1=F;
p11(N)=F(1);
p12(N)=F(2);
p21(N)=F(3);
p22(N)=F(4);
for k=N-1:-1:1,
Pk = A' *Pkplus1*inv(I+E*Pkplus1)*A+Q;
p11 (k) = Pk(1);
p12(k) = Pk(2);
p21(k) = Pk(3);
p22(k) = Pk(4);
Pkplus1 = Pk;
end
For further information, you may check this book. It's great and informative.

Related

Solving system of equations on MATLAB, when a constant exists in variable matrix?

How do I solve the following system of equations on MATLAB when one of the elements of the variable vector is a constant? Please do give the code if possible.
More generally, if the solution is to use symbolic math, how will I go about generating large number of variables, say 12 (rather than just two) even before solving them?
For example, create a number of symbolic variables using syms, and then make the system of equations like below.
syms a1 a2
A = [matrix]
x = [1;a1;a2];
y = [1;0;0];
eqs = A*x == y
sol = solve(eqs,[a1, a2])
sol.a1
sol.a2
In case you have a system with many variables, you could define all the symbols using syms, and solve it like above.
You could also perform a parameter optimization with fminsearch. First you have to define a cost function, in a separate function file, in this example called cost_fcn.m.
function J = cost_fcn(p)
% make sure p is a vector
p = reshape(p, [length(p) 1]);
% system of equations, can be linear or nonlinear
A = magic(12); % your system, I took some arbitrary matrix
sol = A*p;
% the goal of the system of equations to reach, can be zero, or some other
% vector
goal = zeros(12,1);
% calculate the error
error = goal - sol;
% Use a cost criterion, e.g. sum of squares
J = sum(error.^2);
end
This cost function will contain your system of equations, and goal solution. This can be any kind of system. The vector p will contain the parameters that are being estimated, which will be optimized, starting from some initial guess. To do the optimization, you will have to create a script:
% initial guess, can be zeros, or some other starting point
p0 = zeros(12,1);
% do the parameter optimization
p = fminsearch(#cost_fcn, p0);
In this case p0 is the initial guess, which you provide to fminsearch. Then the values of this initial guess will be incremented, until a minimum to the cost function is found. When the parameter optimization is finished, p will contain the parameters that will result in the lowest error for your system of equations. It is however possible that this is a local minimum, if there is no exact solution to the problem.
Your system is over-constrained, meaning you have more equations than unknown, so you can't solve it. What you can do is find a least square solution, using mldivide. First re-arrange your equations so that you have all the constant terms on the right side of the equal sign, then use mldivide:
>> A = [0.0297 -1.7796; 2.2749 0.0297; 0.0297 2.2749]
A =
0.029700 -1.779600
2.274900 0.029700
0.029700 2.274900
>> b = [1-2.2749; -0.0297; 1.7796]
b =
-1.274900
-0.029700
1.779600
>> A\b
ans =
-0.022191
0.757299

Vectorizing the solution of a linear equation system in MATLAB

Summary: This question deals with the improvement of an algorithm for the computation of linear regression.
I have a 3D (dlMAT) array representing monochrome photographs of the same scene taken at different exposure times (the vector IT) . Mathematically, every vector along the 3rd dimension of dlMAT represents a separate linear regression problem that needs to be solved. The equation whose coefficients need to be estimated is of the form:
DL = R*IT^P, where DL and IT are obtained experimentally and R and P must be estimated.
The above equation can be transformed into a simple linear model after applying a logarithm:
log(DL) = log(R) + P*log(IT) => y = a + b*x
Presented below is the most "naive" way to solve this system of equations, which essentially involves iterating over all "3rd dimension vectors" and fitting a polynomial of order 1 to (IT,DL(ind1,ind2,:):
%// Define some nominal values:
R = 0.3;
IT = 600:600:3000;
P = 0.97;
%// Impose some believable spatial variations:
pMAT = 0.01*randn(3)+P;
rMAT = 0.1*randn(3)+R;
%// Generate "fake" observation data:
dlMAT = bsxfun(#times,rMAT,bsxfun(#power,permute(IT,[3,1,2]),pMAT));
%// Regression:
sol = cell(size(rMAT)); %// preallocation
for ind1 = 1:size(dlMAT,1)
for ind2 = 1:size(dlMAT,2)
sol{ind1,ind2} = polyfit(log(IT(:)),log(squeeze(dlMAT(ind1,ind2,:))),1);
end
end
fittedP = cellfun(#(x)x(1),sol); %// Estimate of pMAT
fittedR = cellfun(#(x)exp(x(2)),sol); %// Estimate of rMAT
The above approach seems like a good candidate for vectorization, since it does not utilize MATLAB's main strength that is MATrix operations. For this reason, it does not scale very well and takes much longer to execute than I think it should.
There exist alternative ways to perform this computation based on matrix division, as demonstrated here and here, which involve something like this:
sol = [ones(size(x)),log(x)]\log(y);
That is, appending a vector of 1s to the observations, followed by mldivide to solve the equation system.
The main challenge I'm facing is how to adapt my data to the algorithm (or vice versa).
Question #1: How can the matrix-division-based solution be extended to solve the problem presented above (and potentially replace the loops I am using)?
Question #2 (bonus): What is the principle behind this matrix-division-based solution?
The secret ingredient behind the solution that includes matrix division is the Vandermonde matrix. The question discusses a linear problem (linear regression), and those can always be formulated as a matrix problem, which \ (mldivide) can solve in a mean-square error senseā€”. Such an algorithm, solving a similar problem, is demonstrated and explained in this answer.
Below is benchmarking code that compares the original solution with two alternatives suggested in chat1, 2 :
function regressionBenchmark(numEl)
clc
if nargin<1, numEl=10; end
%// Define some nominal values:
R = 5;
IT = 600:600:3000;
P = 0.97;
%// Impose some believable spatial variations:
pMAT = 0.01*randn(numEl)+P;
rMAT = 0.1*randn(numEl)+R;
%// Generate "fake" measurement data using the relation "DL = R*IT.^P"
dlMAT = bsxfun(#times,rMAT,bsxfun(#power,permute(IT,[3,1,2]),pMAT));
%% // Method1: loops + polyval
disp('-------------------------------Method 1: loops + polyval')
tic; [fR,fP] = method1(IT,dlMAT); toc;
fprintf(1,'Regression performance:\nR: %d\nP: %d\n',norm(fR-rMAT,1),norm(fP-pMAT,1));
%% // Method2: loops + Vandermonde
disp('-------------------------------Method 2: loops + Vandermonde')
tic; [fR,fP] = method2(IT,dlMAT); toc;
fprintf(1,'Regression performance:\nR: %d\nP: %d\n',norm(fR-rMAT,1),norm(fP-pMAT,1));
%% // Method3: vectorized Vandermonde
disp('-------------------------------Method 3: vectorized Vandermonde')
tic; [fR,fP] = method3(IT,dlMAT); toc;
fprintf(1,'Regression performance:\nR: %d\nP: %d\n',norm(fR-rMAT,1),norm(fP-pMAT,1));
function [fittedR,fittedP] = method1(IT,dlMAT)
sol = cell(size(dlMAT,1),size(dlMAT,2));
for ind1 = 1:size(dlMAT,1)
for ind2 = 1:size(dlMAT,2)
sol{ind1,ind2} = polyfit(log(IT(:)),log(squeeze(dlMAT(ind1,ind2,:))),1);
end
end
fittedR = cellfun(#(x)exp(x(2)),sol);
fittedP = cellfun(#(x)x(1),sol);
function [fittedR,fittedP] = method2(IT,dlMAT)
sol = cell(size(dlMAT,1),size(dlMAT,2));
for ind1 = 1:size(dlMAT,1)
for ind2 = 1:size(dlMAT,2)
sol{ind1,ind2} = flipud([ones(numel(IT),1) log(IT(:))]\log(squeeze(dlMAT(ind1,ind2,:)))).'; %'
end
end
fittedR = cellfun(#(x)exp(x(2)),sol);
fittedP = cellfun(#(x)x(1),sol);
function [fittedR,fittedP] = method3(IT,dlMAT)
N = 1; %// Degree of polynomial
VM = bsxfun(#power, log(IT(:)), 0:N); %// Vandermonde matrix
result = fliplr((VM\log(reshape(dlMAT,[],size(dlMAT,3)).')).');
%// Compressed version:
%// result = fliplr(([ones(numel(IT),1) log(IT(:))]\log(reshape(dlMAT,[],size(dlMAT,3)).')).');
fittedR = exp(real(reshape(result(:,2),size(dlMAT,1),size(dlMAT,2))));
fittedP = real(reshape(result(:,1),size(dlMAT,1),size(dlMAT,2)));
The reason why method 2 can be vectorized into method 3 is essentially that matrix multiplication can be separated by the columns of the second matrix. If A*B produces matrix X, then by definition A*B(:,n) gives X(:,n) for any n. Moving A to the right-hand side with mldivide, this means that the divisions A\X(:,n) can be done in one go for all n with A\X. The same holds for an overdetermined system (linear regression problem), in which there is no exact solution in general, and mldivide finds the matrix that minimizes the mean-square error. In this case too, the operations A\X(:,n) (method 2) can be done in one go for all n with A\X (method 3).
The implications of improving the algorithm when increasing the size of dlMAT can be seen below:
For the case of 500*500 (or 2.5E5) elements, the speedup from Method 1 to Method 3 is about x3500!
It is also interesting to observe the output of profile (here, for the case of 500*500):
Method 1
Method 2
Method 3
From the above it is seen that rearranging the elements via squeeze and flipud takes up about half (!) of the runtime of Method 2. It is also seen that some time is lost on the conversion of the solution from cells to matrices.
Since the 3rd solution avoids all of these pitfalls, as well as the loops altogether (which mostly means re-evaluation of the script on every iteration) - it unsurprisingly results in a considerable speedup.
Notes:
There was very little difference between the "compressed" and the "explicit" versions of Method 3 in favor of the "explicit" version. For this reason it was not included in the comparison.
A solution was attempted where the inputs to Method 3 were gpuArray-ed. This did not provide improved performance (and even somewhat degradaed them), possibly due to wrong implementation, or the overhead associated with copying matrices back and forth between RAM and VRAM.

Solving for Fisher Kolmagorov Partial Differential Equation

I have been trying to solve the non dimensional Fisher Kolmagorov equation in Matlab. I am getting a graph which doesn't look at all like it should. Also, I'm getting the equation independent of value of s (the source term in the pdepe solver). No matter what value of s I put in the graph remains the same.
function FK
m = 0;
x = linspace(0,1,100);
t = linspace(0,1,100);
u = pdepe(m,#FKpde,#FKic,#FKbc,x,t);
[X,T] = meshgrid(x,t);
%ANALYTICAL SOLUTION
% a=(sqrt(2))-1;
% q=2;
% s=2/q;
% b= q /((2*(q+2))^0.5);
% c= (q+4)/((2*(q+2))^0.5);
% zeta= X-c*T;
%y = 1/((1+(a*(exp(b*zeta))))^s);
%r=(y(:,:)-u(:,:))./y(:,:); % relative error in numerical and analytical value
figure;
plot(x,u(10,:),'o',x,u(40,:),'o',x,u(60,:),'o',x,u(end,:),'o')
title('Numerical Solutions at different times');
legend('tn=1','tn=5','tn=30','tn=10','ta=20','ta=600','ta=800','ta=1000',0);
xlabel('Distance x');
ylabel('u(x,t)');
% --------------------------------------------------------------------------
function [c,f,s] = FKpde(x,t,u,DuDx)
c = 1;
f = DuDx;
s =u*(1-u);
% --------------------------------------------------------------------------
function u0 = FKic(x)
u0 = 10^(-4);
% --------------------------------------------------------------------------
function [pl,ql,pr,qr] = FKbc(xl,ul,xr,ur,t)
pl = ul-1;
ql = 0;
pr = ur;
qr = 0;
Should maybe be a comment, but putting it as an answer for better formatting. Your analytic solution, which I assume you're using to compare with the numerical answer to say that the graph does not look as it should, does not appear to respect the initial conditions or boundary conditions you are feeding pdepe, so I'd start there if trying to figure out why u does not look like y
The initial and boundary conditions you are setting are:
u(0, t) = 1
u(1, t) = 0
u(x, 0) = 1e-4
Setting aside that the boundary and initial conditions are in conflict, the analytic solution you suggest in the code has
u(0, t) = 1/((1+exp(-b*ct)))
u(1, t) = 1/((1+exp(b*(1-ct)))
u(x, 0) = 1/((1+exp(b*x))
So it seems to me the numerical and analytic solutions should be expected to be different, and the differences you observe are probably due to the IC/BC setup. I suspect that pdepe is probably solving the equation you are giving it.
On increasing the length scale and time scale, I get the answers I want. The problem was to solve for different times, and see the wave propogate. For small lenghts, I could only see part of that wave.

Issues in fitting data to linear model

Assuming a noiseless AR(1) process y(t)= a*y(t-1) . I have following conceptual questions and shall be glad for the clarification.
Q1 - Discrepancy between mathematical formulation and implementation - The mathematical formulation of AR model is in the form of y(t) = - summmation over i=1 to p[a*y(t-p)] + eta(t) where p=model order and eta(t) is a white gaussian noise. But when estimating coefficients using any method like arburg() or the least square, we simply call that function. I do not know if a white gaussian noise is implicitly added. Then, when we resolve the AR equation with the estimated coefficients, I have seen that the negative sign is not considered nor the noise term added.
What is the correct representation of AR model and how do I find the average coefficients over k number of trials when I have only a single sample of 1000 data points?
Q2 - Coding problem in How to simulate fitted_data for k number of trials and then find the residuals - I fitted a data "data" generated from unknown system and obtained the coefficient by
load('data.txt');
for trials = 1:10
model = ar(data,1,'ls');
original_data=data;
fitted_data(i)=coeff1*data(i-1); % **OR**
data(i)=coeff1*data(i-1);
fitted_data=data;
residual= original_data - fitted_data;
plot(original_data,'r'); hold on; plot(fitted_data);
end
When calculating residual is the fitted_data obtained as above by resolving the AR equation with the obtained coefficients? Matlab has a function for doing this but I wanted to make my own. So, after finding coefficients from the original data how do I resolve ? The coding above is incorrect. Attached is the plot of original data and the fitted_data.
If you model is simply y(n)= a*y(n-1) with scalar a, then here is the solution.
y = randn(10, 1);
a = y(1 : end - 1) \ y(2 : end);
y_estim = y * a;
residual = y - y_estim;
Of course, you should separate the data into train-test, and apply a on the test data. You can generalize this approach to y(n)= a*y(n-1) + b*y(n-2), etc.
Note that \ represents mldivide() function: mldivide
Edit:
% model: y[n] = c + a*y(n-1) + b*y(n-2) +...+z*y(n-n_order)
n_order = 3;
allow_offset = true; % alows c in the model
% train
y_train = randn(20,1); % from your data
[y_in, y_out] = shifted_input(y_train, n_order, allow_offset);
a = y_in \ y_out;
% now test
y_test = randn(20,1); % from your data
[y_in, y_out] = shifted_input(y_test, n_order, allow_offset);
y_estim = y_in * a; % same a
residual = y_out - y_estim;
here is shifted_input():
function [y_in, y_out] = shifted_input(y, n_order, allow_offset)
y_out = y(n_order + 1 : end);
n_rows = size(y, 1) - n_order;
y_in = nan(n_rows, n_order);
for k = 1 : n_order
y_in(:, k) = y(1 : n_rows);
y = circshift(y, -1);
end
if allow_offset
y_in = [y_in, ones(n_rows, 1)];
end
return
AR-type models can serve a number of purposes, including linear prediction, linear predictive coding, filtering noise. The eta(t) are not something we are interested in retaining, rather part of the point of the algorithms is to remove their influence to any extent possible by looking for persistent patterns in the data.
I have textbooks that, in the context of linear prediction, do not include the negative sign included in your expression prior to the sum. On the other hand Matlab's function lpcdoes:
Xp(n) = -A(2)*X(n-1) - A(3)*X(n-2) - ... - A(N+1)*X(n-N)
I recommend you look at function lpc if you haven't already, and at the examples from the documentation such as the following:
randn('state',0);
noise = randn(50000,1); % Normalized white Gaussian noise
x = filter(1,[1 1/2 1/3 1/4],noise);
x = x(45904:50000);
% Compute the predictor coefficients, estimated signal, prediction error, and autocorrelation sequence of the prediction error:
p = lpc(x,3);
est_x = filter([0 -p(2:end)],1,x); % Estimated signal
e = x - est_x; % Prediction error
[acs,lags] = xcorr(e,'coeff'); % ACS of prediction error
The estimated x is computed as est_x. Note how the example uses filter. Quoting the matlab doc again, filter(b,a,x) "is a "Direct Form II Transposed" implementation of the standard difference equation:
a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb)
- a(2)*y(n-1) - ... - a(na+1)*y(n-na)
which means that in the prior example est_x(n) is computed as
est_x(n) = -p(2)*x(n-1) -p(3)*x(n-2) -p(4)*x(n-3)
which is what you expect!
Edit:
As regards the function ar, the matlab documentation explains that the output coefficients have the same meaning as in the lp scenario discussed above.
The right way to evaluate the output of the AR model is to compute
data_armod(i)= -coeff(2)*data(i-1) -coeff(3)*data(i-2) -coeff(4)*data(i-3)
where coeff is the coefficient matrix returned with
model = ar(data,3,'ls');
coeff = model.a;

Matlab: finding coefficients of ODE system

I have all the data and an ODE system of three equations which has 9 unknown coefficients (a1, a2,..., a9).
dS/dt = a1*S+a2*D+a3*F
dD/dt = a4*S+a5*D+a6*F
dF/dt = a7*S+a8*D+a9*F
t = [1 2 3 4 5]
S = [17710 18445 20298 22369 24221]
D = [1357.33 1431.92 1448.94 1388.33 1468.95]
F = [104188 104792 112097 123492 140051]
How to find these coefficients (a1,..., a9) of an ODE using Matlab?
I can't spend too much time on this, but basically you need to use math to reduce the equation to something more meaningful:
your equation is of the order
dx/dt = A*x
ergo the solution is
x(t-t0) = exp(A*(t-t0)) * x(t0)
Thus
exp(A*(t-t0)) = x(t-t0) * Pseudo(x(t0))
Pseudo is the Moore-Penrose Pseudo-Inverse.
EDIT: Had a second look at my solution, and I didn't calculate the pseudo-inverse properly.
Basically, Pseudo(x(t0)) = x(t0)'*inv(x(t0)*x(t0)'), as x(t0) * Pseudo(x(t0)) equals the identity matrix
Now what you need to do is assume each time step (1 to 2, 2 to 3, 3 to 4) is an experiment (therefore t-t0=1), so the solution would be to:
1- Build your pseudo inverse:
xt = [S;D;F];
xt0 = xt(:,1:4);
xInv = xt0'*inv(xt0*xt0');
2- Get exponential result
xt1 = xt(:,2:5);
expA = xt1 * xInv;
3- Get the logarithm of the matrix:
A = logm(expA);
And since t-t0= 1, A is our solution.
And a simple proof to check
[t, y] = ode45(#(t,x) A*x,[1 5], xt(1:3,1));
plot (t,y,1:5, xt,'x')
You have a linear, coupled system of ordinary differential equations,
y' = Ay with y = [S(t); D(t); F(t)]
and you're trying to solve the inverse problem,
A = unknown
Interesting!
First line of attack
For given A, it is possible to solve such systems analytically (read the wiki for example).
The general solution for 3x3 design matrices A take the form
[S(t) D(t) T(t)].' = c1*V1*exp(r1*t) + c2*V2*exp(r2*t) + c3*V3*exp(r3*t)
with V and r the eigenvectors and eigenvalues of A, respectively, and c scalars that are usually determined by the problem's initial values.
Therefore, there would seem to be two steps to solve this problem:
Find vectors c*V and scalars r that best-fit your data
reconstruct A from the eigenvalues and eigenvectors.
However, going down this road is treaturous. You'd have to solve the non-linear least-squares problem for the sum-of-exponentials equation you have (using lsqcurvefit, for example). That would give you vectors c*V and scalars r. You'd then have to unravel the constants c somehow, and reconstruct the matrix A with V and r.
So, you'd have to solve for c (3 values), V (9 values), and r (3 values) to build the 3x3 matrix A (9 values) -- that seems too complicated to me.
Simpler method
There is a simpler way; use brute-force:
function test
% find
[A, fval] = fminsearch(#objFcn, 10*randn(3))
end
function objVal = objFcn(A)
% time span to be integrated over
tspan = [1 2 3 4 5];
% your desired data
S = [17710 18445 20298 22369 24221 ];
D = [1357.33 1431.92 1448.94 1388.33 1468.95 ];
F = [104188 104792 112097 123492 140051 ];
y_desired = [S; D; F].';
% solve the ODE
y0 = y_desired(1,:);
[~,y_real] = ode45(#(~,y) A*y, tspan, y0);
% objective function value: sum of squared quotients
objVal = sum((1 - y_real(:)./y_desired(:)).^2);
end
So far so good.
However, I tried both the complicated way and the brute-force approach above, but I found it very difficult to get the squared error anywhere near satisfyingly small.
The best solution I could find, after numerous attempts:
A =
1.216731997197118e+000 2.298119167536851e-001 -2.050312097914556e-001
-1.357306715497143e-001 -1.395572220988427e-001 2.607184719979916e-002
5.837808840775175e+000 -2.885686207763313e+001 -6.048741083713445e-001
fval =
3.868360951628554e-004
Which isn't bad at all :) But I would've liked a solution that was less difficult to find...