# Gaussian-Process based MPC Inverted Pendulum
- Overall structure
## MotionModelGP_InvPendulum_nominal.m
- 繼承 [MotionModelGP](#MotionModelGP.m)
```typescript=
classdef MotionModelGP_InvPendulum_nominal < MotionModelGP
```
- 定義類的特性
*聲明 constant 後無法隨意更改其值
```typescript=
properties
Mc % mass of the carriage
Mp % mass of the pole
b % friction coefficient between the carriage and the floor
I % inertia matrix of the pole CG
l % pole length
g = 9.8
end
properties(Constant)
% keep in mind the dimensions: xk+1 = fd(xk,uk) + Bd*(d(z)+w)),
% where z = [Bz_x*x;Bz_u*u]
Bz_x = [0 0 1 0
0 0 0 1]
Bz_u = [];
Bd = [0; % xk+1 = fd(xk,uk) + Bd*d(zk)
0;
1;
0]
n = 4 % number of outputs x(t)
m = 1 % number of inputs u(t)
nz = 2 % dimension of z(t)
nd = 1 % output dimension of d(z)
end
```
- 對object進行操作的方法(函數)
```typescript=
methods
```
- 建立標稱(無干擾動態)系統之GP模型object，並定義倒單擺參數
```typescript=
function obj = MotionModelGP_InvPendulum_nominal (Mc, Mp, b, I, l, d, sigmaw)
obj = obj @ MotionModelGP(d,sigmaw);
% store parameters
obj.Mc = Mc;
obj.Mp = Mp;
obj.b = b;
obj.I = I;
obj.l = l;
end
```
- 建立系統動態微分方程 $\dot{x}=f(x,u)$
```typescript=
function xdot = f (obj, x, u)
params = [obj.Mc obj.Mp obj.I obj.g obj.l obj.b]';
xdot = invertedPendulum_f(x, u, params );
end
```
- Jacobian of autonomous system $\nabla f_x$
```typescript=
function gradx = gradx_f(obj, x, u)
params = [obj.Mc obj.Mp obj.I obj.g obj.l obj.b]';
gradx = invertedPendulum_gradx_f(x, u, params );
end
```
- Jacobian of input effect $\nabla f_u$
```typescript=
function gradu = gradu_f(obj, x, u)
params = [obj.Mc obj.Mp obj.I obj.g obj.l obj.b]';
gradu = invertedPendulum_gradu_f(x, u, params );
end
```
- Linearized model
```typescript=
function [A,B] = linearize (obj)
Mc=obj.Mc; Mp=obj.Mp; b=obj.b; I=obj.I; l=obj.l; g=obj.g;
p = I*(Mc+Mp)+Mc*Mp*l^2;
A = [0 1 0 0;
0 -(I+Mp*l^2)*b/p (Mp^2*g*l^2)/p 0;
0 0 0 1;
0 -(Mp*l*b)/p Mp*g*l*(Mc+Mp)/p 0];
B = [ 0;
(I+Mp*l^2)/p;
0;
Mp*l/p];
end
```
- 靜態方法(不需要以object為輸入參數，不創建類的對象也可調用此方法)
```typescript=
methods(Static)
```
- Generate .m file for continuous time dynamics equations
include $\dot{x}=f(x,u), \nabla f_x, \nabla f_u$
```typescript=
function generate_invertedPendulum_functions()
syms g Mc Mp b I l F T s ds dds th dth ddth real
T = 0; % we are not using this input for now
fzero = [(Mc+Mp)*dds + b*ds + Mp*l/2*ddth*cos(th) - Mp*l/2*dth^2*sin(th) - F ;
(I+Mp*(l/2)^2)*ddth + Mp*g*l/2*sin(th) + Mp*l*dds*cos(th) - T ];
sol = solve(fzero,[dds,ddth]);
dds = simplify(sol.dds);
ddth = simplify(sol.ddth);
u = F;
x = [s, ds, th, dth]';
xdot = [ds, dds, dth, ddth]';
params = [Mc Mp I g l b ]';
folder = fullfile(pwd,'CODEGEN');
if ~exist(folder,'dir')
mkdir(folder);
end
addpath(folder)
matlabFunction( xdot, 'Vars', {x;u;params} ,'File', fullfile('CODEGEN','invertedPendulum_f') );
gradx_f = simplify(jacobian(xdot,x)');
matlabFunction( gradx_f, 'Vars', {x;u;params} ,'File', fullfile('CODEGEN','invertedPendulum_gradx_f') );
gradu_f = simplify(jacobian(xdot,u)');
matlabFunction( gradu_f, 'Vars', {x;u;params} ,'File', fullfile('CODEGEN','invertedPendulum_gradu_f') );
disp('FINISHED! functions invertedPendulum_f, invertedPendulum_gradx_f and invertedPendulum_gradu_f generated!!')
end
```
### MotionModelGP_InvPendulum_deffect.m
- 繼承 [MotionModelGP_InvPendulum_nominal](##MotionModelGP_InvPendulum_nominal.m)
```typescript=
classdef MotionModelGP_InvPendulum_deffect < MotionModelGP_InvPendulum_nominal
```
- 建立object方式與nominal相同
```typescript=
methods
function obj = MotionModelGP_InvPendulum_deffect(Mc, Mp, b, I, l, d, sigmaw)
obj = obj @ MotionModelGP_InvPendulum_nominal(Mc, Mp, b, I, l, d, sigmaw);
end
```
- 對 $x_{3}=\theta$ 加入干擾
```typescript=
function xdot = f (obj, x, u)
% get dynamics from nominal model
xdot = f @ MotionModelGP_InvPendulum_nominal(obj,x,u);
% add deffect
xdot(3) = xdot(3) + (0.1 * x(3) - 0.01*x(4) + deg2rad(3)) *10;
end
```
## MotionModelGP.m
- 默認value類不支援在類內的構造函數以外的函數中修改屬性，handle反之
```typescript=
classdef (Abstract) MotionModelGP < handle
```
- 設定矩陣維度參數、離散化方法(private無法透過外部方法直接修改)
```typescript=
properties (Abstract, Constant)
Bd % <n,xx> xk+1 = fd(xk,uk) + Bd*d(zk)
Bz_x % <yy,n> z = [Bz_x*x;Bz_u*u]
Bz_u % <ww,n>
n % <1> number of outputs x(t)
m % <1> number of inputs u(t)
nd % <1> output dimension of d(z)
nz % <1> dimension of z(t)
end
properties (SetAccess=private)
discretization = 'ode2';
d % [E[d(z)] , Var[d(z)]] = d(z): disturbace model
var_w % measurement noise covariance matrix. w ~ N(0,var_w)
end
properties (SetAccess=public)
% is_d_active = false
end
```
- 根據 process noise covarience 及 disturbance 建立 obj
```typescript=
function obj = MotionModelGP (d, var_w)
obj.d = d;
if isempty(obj.d)
mu_d = @(z) zeros(obj.nd,1);
var_d = @(z) zeros(obj.nd);
obj.d = @(z) deal( mu_d(z), var_d(z) );
end
obj.var_w = var_w;
if isempty(obj.var_w)
obj.var_w = zeros(obj.nd);
end
% assert model
...
...
end
```
- 量測狀態
```typescript=
function zk = z(obj, xk, uk)
if ~isempty(obj.Bz_x)
z_xk = obj.Bz_x * xk; else, z_xk=[];
end
if ~isempty(obj.Bz_u)
z_uk = obj.Bz_u * uk; else, z_uk = [];
end
zk = [ z_xk ; z_uk ];
end
```
- Select discretization method then predict states and jacobian at next step
```typescript=
function [xkp1, gradx_xkp1] = fd (obj, xk, uk, dt)
%------------------------------------------------------------------
% Discrete time dynamics (ODE1 Euler approximation)
% args:
% xkp1: <n,1> state prediction (without disturbance model)
% grad_xkp1: <n,n> gradient of xkp1 w.r.t. xk
%------------------------------------------------------------------
if strcmp(obj.discretization,'ode1')
%-----------------
% Fixed step ODE1
%-----------------
% calculate continous time dynamics
xkp1 = xk + dt * obj.f(xk,uk);
elseif strcmp(obj.discretization,'ode2')
%-----------------
% Fixed step ODE2 (developed by myself)
%-----------------
[~,xkp1] = ODE.ode2(@(t,x) obj.f(x,uk), xk, dt, dt);
elseif strcmp(obj.discretization,'ode4')
%-----------------
% Fixed step ODE4 (developed by myself)
%-----------------
[~,xkp1] = ODE.ode4(@(t,x) obj.f(x,uk), xk, dt, dt);
elseif strcmp(obj.discretization,'ode23')
%-----------------
% Variable step ODE23
%-----------------
opts_1 = odeset('Stats','off','RelTol',1e-1,'AbsTol',1e-1);
[~,xkp1_23] = ode23( @(t,x) obj.f(x,uk), [0 dt], xk, opts_1);
xkp1 = xkp1_23(end,:)';
elseif strcmp(obj.discretization,'ode23')
%-----------------
% Variable step ODE23
%-----------------
opts_1 = odeset('Stats','off','RelTol',1e-1,'AbsTol',1e-1);
[~,xkp1_23] = ode23( @(t,x) obj.f(x,uk), [0 dt], xk, opts_1);
xkp1 = xkp1_23(end,:)';
else
error('Chosen discretization: %s is not yet implemented',obj.discretization);
end
% for now, gradient is being discretized using a simple ode1
gradx_xdot = obj.gradx_f(xk,uk);
gradx_xkp1 = eye(obj.n) + dt * gradx_xdot;
end
```
- 預測狀態之期望值及協方差
```typescript=
function [mu_xkp1,var_xkp1] = xkp1 (obj, mu_xk, var_xk, uk, dt)
% calculate discrete time dynamics
[fd, gradx_fd] = obj.fd(mu_xk,uk,dt);
% calculate grad_{x,d,w} xk+1
grad_xkp1 = [gradx_fd; obj.Bd'; obj.Bd'];
% select variables (xk,uk) -> z
z = obj.z(mu_xk,uk);
% evaluate disturbance
[mu_d, var_d] = obj.d(z);
% A) Mean Equivalent Approximation:
var_x_d_w = blkdiag(var_xk, var_d, obj.var_w);
% predict mean and variance (Extended Kalman Filter)
mu_xkp1 = fd + obj.Bd * ( mu_d );
var_xkp1 = grad_xkp1' * var_x_d_w * grad_xkp1; % zeros(obj.n);
end
```
## GP.m
- 定義GP object屬性
```typescript=
properties
isActive = true
end
properties(SetAccess=private)
% kernel parameters (one for each output dimension)
M % <n,n,p> length scale covariance
var_f % <p> signal/output covariance
var_n % <p> evaluation noise covariance
% dictionary: [X,Y]
X % <n,N> input dataset
Y % <N,p> output dataset
N % <1> dictionary size
Nmax % <1> max dictionary size
n % <1> input dimension
p % <1> output dimension
% aux variables
L % <N,N,p>: chol(obj.KXX + sigman^2 * I,'lower');
alpha % <N,p>: L'\(L\(Y-muX));
% (DEPRECATED) inv_KXX_sn % <N,N>
% isOutdated = false % <bool> model is outdated if data has been added withouth updating L and alpha matrices
% isOptimized = false % <bool> true if model had its kernel parameters optimized and no new data has been added
end
```
- GP object 設置
```typescript=
function obj = GP(n, p, var_f, var_n, M, maxsize)
%------------------------------------------------------------------
% GP constructor
% args:
% n: <1> input dimension
% p: <1> output dimension
% var_f: <p> signal/output covariance
% var_n: <p> evaluation noise covariance
% M: <n,n,p> length scale covariance matrix
% maxsize: <1> maximum dictionary size
%------------------------------------------------------------------
obj.n = n;
obj.p = p;
obj.X = [];
obj.Y = [];
obj.Nmax = maxsize;
obj.setHyperParameters( M, var_f, var_n )
end
```
- GP 超參數向量
```typescript=
function setHyperParameters(obj, M, var_f, var_n)
%------------------------------------------------------------------
% set new values to the hyperparameter
%------------------------------------------------------------------
assert( obj.n == size(M,1), 'Matrix M has wrong dimension or parameters n/p are wrong. Expected dim(M)=<n,n,p>=<%d,%d,%d>',obj.n,obj.n,obj.p);
assert( obj.p == size(var_f,1), 'Matrix var_f has wrong dimension or parameter p is wrong. Expected dim(var_f)=<p>=<%d>, got <%d>.',obj.p,size(var_f,1));
assert( obj.p == size(var_n,1), 'Matrix var_n has wrong dimension or parameter p is wrong. Expected dim(var_n)=<p>=<%d>, got <%d>.',obj.p,size(var_n,1));
obj.M = M;
obj.var_f = var_f;
obj.var_n = var_n;
obj.updateModel();
end
```
- GP kernel
```typescript=
function kernel = K(obj, x1, x2)
%------------------------------------------------------------------
% SEQ kernel function: cov[f(x1),f(x2)]
% k(x1,x2) = var_f * exp( 0.5 * ||x1-x2||^2_M )
% args:
% x1: <n,N1>
% x2: <n,N2>
% out:
% kernel: <N1,N2,p>
%------------------------------------------------------------------
kernel = zeros(size(x1,2),size(x2,2),obj.p);
for pi = 1:obj.p
D = pdist2(x1',x2','mahalanobis',obj.M(:,:,pi)).^2;
%D = pdist2(x1',x2','seuclidean',diag((obj.M).^0.5)).^2;
kernel(:,:,pi) = obj.var_f(pi) * exp( -0.5 * D );
end
end
```
- GP regression model implementation
```typescript=
function updateModel(obj)
% -----------------------------------------------------------------
% Update precomputed matrices L and alpha, that will be used when
% evaluating new points. See [Rasmussen, pg19].
% -----------------------------------------------------------------
% nothing to update... return
if obj.N == 0
return;
end
% store cholesky L and alpha matrices
I = eye(obj.N);
% for each output dimension
obj.alpha = zeros(obj.N,obj.p);
obj.L = zeros(obj.N,obj.N);
K = obj.K(obj.X,obj.X);
for pi=1:obj.p
obj.L(:,:,pi) = chol(K(:,:,pi)+ obj.var_n(pi) * I ,'lower');
% sanity check: norm( L*L' - (obj.K(obj.X,obj.X) + obj.var_n*I) ) < 1e-12
obj.alpha(:,pi) = obj.L(:,:,pi)'\(obj.L(:,:,pi)\(obj.Y(:,pi)-obj.mu(obj.X)));
end
%-------------------- (DEPRECATED) ------------------------
% % SLOW BUT RETURNS THE FULL COVARIANCE MATRIX INSTEAD OF ONLY THE DIAGONAL (VAR)
% % precompute inv(K(X,X) + sigman^2*I)
% I = eye(obj.N);
% obj.inv_KXX_sn = inv( obj.K(obj.X,obj.X) + obj.var_n * I );
%-------------------- (DEPRECATED) ------------------------
end
```
- GP資料點前處理(Input & Output dataset)
```typescript=
function add(obj, X, Y)
%------------------------------------------------------------------
% Add new data points [X,Y] to the dictionary
%
% args:
% X: <n,N>
% Y: <N,p>
%------------------------------------------------------------------
OPTION = 'B'; % {'A','B'}
assert(size(Y,2) == obj.p, ...
sprintf('Y should have %d columns, but has %d. Dimension does not agree with the specified kernel parameters',obj.p,size(Y,2)));
assert(size(X,1) == obj.n, ...
sprintf('X should have %d rows, but has %d. Dimension does not agree with the specified kernel parameters',obj.n,size(X,1)));
Ntoadd = size(X,2);
Nextra = obj.N + Ntoadd - obj.Nmax;
% if there is space enough to append the new data points, then
if Nextra <= 0
obj.X = [obj.X, X];
obj.Y = [obj.Y; Y];
% data overflow: dictionary will be full. we need to select
% relevant points
else
Nthatfit = obj.Nmax - obj.N;
% make dictionary full
obj.X = [obj.X, X(:,1:Nthatfit) ];
obj.Y = [obj.Y; Y(1:Nthatfit,:) ];
obj.updateModel();
% points left to be added
X = X(:,Nthatfit+1:end);
Y = Y(Nthatfit+1:end,:);
% OPTION A)
% The closest (euclidian dist.) points will be iteratively removed
if strcmp(OPTION,'A')
for i=1:Nextra
D = pdist2(obj.X',X(:,i)','euclidean').^2;
[~,idx_rm] = min(D);
idx_keep = 1:obj.N ~= idx_rm;
obj.X = [obj.X(:,idx_keep), X(:,i)];
obj.Y = [obj.Y(idx_keep,:); Y(i,:)];
end
% OPTION B)
% the point with lowest variance will be removed
elseif strcmp(OPTION,'B')
X_all = [obj.X,X];
Y_all = [obj.Y;Y];
[~, var_y] = obj.eval( X_all, true);
[~,idx_keep] = maxk(sum(reshape(var_y, obj.p^2, obj.N+Nextra )),obj.Nmax);
obj.X = X_all(:,idx_keep);
obj.Y = Y_all(idx_keep,:);
else
error('Option not implemented');
end
end
% update pre-computed matrices
obj.updateModel();
end
```
- 計算GP之期望值及協方差
```typescript=
function [mu_y, var_y] = eval(obj, x, varargin)
%------------------------------------------------------------------
% Evaluate GP at the points x
% This is a fast implementation of [Rasmussen, pg19]
%
% args:
% x: <n,Nx> point coordinates
% varargin:
% true: force calculation of mean and variance even if GP is inactive
% out:
% muy: <p,Nx> E[Y] = E[gp(x)]
% vary: <p,p,Nx> Var[Y] = Var[gp(x)]
%------------------------------------------------------------------
assert(size(x,1)==obj.n, sprintf('Input vector has %d columns but should have %d !!!',size(x,1),obj.n));
% calculate mean and variance even if GP is inactive
forceActive = length(varargin)>=1 && varargin{1}==true;
Nx = size(x,2); % size of dataset to be evaluated
% if there is no data in the dictionary or GP is not active
% then return prior (for now returning zero variance)
if obj.N == 0 || (~obj.isActive && ~forceActive)
mu_y = repmat(obj.mu(x),[1,obj.p])';
var_y = zeros(obj.p,obj.p,Nx);
return;
end
% in case the matrices alpha and L are empty we need to update the model
if isempty(obj.alpha) || isempty(obj.L)
obj.updateModel();
end
% Calculate posterior mean mu_y for each output dimension
KxX = obj.K(x,obj.X);
mu_y = zeros(obj.p,Nx);
for pi=1:obj.p
mu_y(pi,:) = obj.mu(x) + KxX(:,:,pi) * obj.alpha(:,pi);
end
% Calculate posterior covariance var_y
var_y = zeros(obj.p,obj.p,Nx);
for pi=1:obj.p
for i=1:Nx
% (less efficient) v = obj.L\obj.K(x(:,i),obj.X)';
v = obj.L(:,:,pi)\KxX(i,:,pi)';
K = obj.K(x(:,i),x(:,i));
var_y(pi,pi,i) = K(:,:,pi) - v'*v;
end
end
% --------------------- (DEPRECATED) -------------------------
% % SLOW BUT RETURNS THE FULL COVARIANCE MATRIX INSTEAD OF ONLY THE DIAGONAL (VAR)
% KxX = obj.K(x,obj.X);
% muy = obj.mu(x) + KxX * obj.inv_KXX_sn * (obj.Y-obj.mu(obj.X));
% vary = obj.K(x,x) - KxX * obj.inv_KXX_sn * KxX';
% --------------------- (DEPRECATED) -------------------------
end
```
- GP 超參數優化 - Maximum Log Likelihood
```typescript=
function optimizeHyperParams(obj, method)
%------------------------------------------------------------------
% Optimize kernel hyper-parameters based on the current dictionary
% Method: maximum log likelihood (See Rasmussen's book Sec. 5.4.1)
%------------------------------------------------------------------
warning('off', 'MATLAB:nearlySingularMatrix')
warning('off', 'MATLAB:singularMatrix')
obj.updateModel();
% error('not yet implemented!!!');
for ip = 1:obj.p
% set optimization problem
nvars = obj.n + 1 + 1; % M, var_f, var_n
%fun = @(vars) loglikelihood(obj,ip,vars);
fun = @(vars) loglikelihood(obj,ip,... % output dim
diag(10.^vars(1:end-2)),... % M
10.^vars(end-1),... % var_f
10.^vars(end)); % var_n
ub = [ 1e+5 * ones(obj.n,1);
1e+5;
1e+5 ];
lb = [ 1e-8 * ones(obj.n,1);
0*1e-8;
1e-10 ];
x0 = [ diag(obj.M(:,:,ip)); obj.var_f(ip); obj.var_n(ip); ];
% convert to log10 space
ub = log10(ub);
lb = log10(lb);
x0 = log10(x0);
% use genetic algorithm or interior-point-method
if strcmp(method, 'ga')
options = optimoptions('ga',...
'ConstraintTolerance',1e-6,...
'PlotFcn', @gaplotbestf,...
'Display','iter',...
'UseParallel',false);
opt_vars = ga(fun,nvars,[],[],[],[],lb,ub,[],[],options);
elseif strcmp(method,'fmincon')
options = optimoptions('fmincon', ...
'PlotFcn','optimplotfval',...
'Display','iter');
[opt_vars,~] = fmincon(fun,x0,[],[],[],[],lb,ub,[],options);
else
error('Method %s not implemented, please choose an existing method',method);
end
% retrieve optimal results to absolute scale
obj.M(:,:,ip) = diag( 10.^opt_vars(1:end-2) );
obj.var_f(ip) = 10.^opt_vars(end-1);
obj.var_n(ip) = 10.^opt_vars(end);
end
% update matrices alpha and L
obj.updateModel();
warning('on', 'MATLAB:nearlySingularMatrix')
warning('on', 'MATLAB:singularMatrix')
end
function logL = loglikelihood(obj, outdim, M, var_f, var_n)
%------------------------------------------------------------------
% calculate the negative log likelihood: -log(p(Y|X,theta)),
% where theta are the hyperparameters and (X,Y) the training data
%------------------------------------------------------------------
Y = obj.Y(:,outdim);
K = var_f * exp( -0.5 * pdist2(obj.X',obj.X','mahalanobis',M).^2 );
Ky = K + var_n*eye(obj.N);
% calculate log(p(Y|X,theta))
logL = -(-0.5*Y'/Ky*Y -0.5*logdet(Ky) -obj.n/2*log(2*pi));
end
```
## NMPC.m
*create by author, not built-in function
Sign in with Wallet
Connect another wallet