Skip to content

Commit bac1db5

Browse files
LuziaKnLuziaKn
committed
Merge remote-tracking branch 'origin/test_ex_singletrack' into test_ex_singletrack
2 parents 33db1eb + 4f1a26f commit bac1db5

13 files changed

+547
-128
lines changed

‎GP.m

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -116,7 +116,7 @@
116116
for pi = 1:obj.p
117117
D(:,:,pi) = pdist2(x1',x2','mahalanobis',obj.M(:,:,pi)).^2;
118118
%D = pdist2(x1',x2','seuclidean',diag((obj.M).^0.5)).^2;
119-
kernel(:,:,pi) = obj.var_f(pi) * exp( -0.5 * D(:,:,pi) );
119+
kernel(:,:,pi) = obj.var_f(pi) * exp( -0.5 * D(:,:,pi) );
120120
end
121121
end
122122

@@ -185,9 +185,15 @@ function add(obj,X,Y)
185185
% dictionary is full
186186
if obj.N + size(X,2) > obj.Nmax
187187
% For now, we just keep the most recent data
188-
obj.X = [obj.X(:,2:end), X]; % concatenation in the 2st dim.
189-
obj.Y = [obj.Y(2:end,:); Y]; % concatenation in the 1st dim.
190-
188+
% obj.X = [obj.X(:,2:end), X]; % concatenation in the 2st dim.
189+
% obj.Y = [obj.Y(2:end,:); Y]; % concatenation in the 1st dim.
190+
191+
D = pdist2(obj.X',X','mahalanobis', eye(5) ).^2;
192+
[~,idx] = max(D);
193+
194+
obj.X = [obj.X(:,1:obj.N ~= idx), X]; % concatenation in the 2st dim.
195+
obj.Y = [obj.Y(1:obj.N ~= idx,:); Y]; % concatenation in the 1st dim.
196+
191197
% append to dictionary
192198
else
193199
obj.X = [obj.X, X]; % concatenation in the 2st dim.

‎MotionModelGP.m

Lines changed: 32 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -69,10 +69,21 @@
6969
% d: evaluates nonlinear motion model mean and covariance
7070
% function [mean_d, var_d] = d(z), with z = Bz*x
7171
% var_w: <1> measurement noise covariance
72+
% obs: if d or var_w are empty, then this function will set them
73+
% to zero with the correct dimensions
7274
%------------------------------------------------------------------
7375
obj.d = d;
76+
if isempty(obj.d)
77+
mu_d = @(z) zeros(obj.nd,1);
78+
var_d = @(z) zeros(obj.nd);
79+
obj.d = @(z) deal( mu_d(z), var_d(z) );
80+
end
81+
7482
obj.var_w = var_w;
75-
83+
if isempty(obj.var_w)
84+
obj.var_w = zeros(obj.nd);
85+
end
86+
7687
%--------------------------------------------------------------
7788
% assert model
7889
%--------------------------------------------------------------
@@ -81,8 +92,8 @@
8192
assert(size(obj.Bd,2) == obj.nd, ...
8293
sprintf('obj.Bd matrix should have %d columns, but has %d',obj.nd,size(obj.Bd,2)))
8394

84-
assert( all(size(var_w)==[obj.nd,obj.nd]), ...
85-
sprintf('Variable var_w should have dimension %d, but has %d',obj.nd,size(var_w,1)))
95+
assert( all(size(obj.var_w)==[obj.nd,obj.nd]), ...
96+
sprintf('Variable var_w should have dimension %d, but has %d',obj.nd,size(obj.var_w,1)))
8697
assert(size(obj.Bd,1) == obj.n, ...
8798
sprintf('obj.Bd matrix should have %d rows, but has %d',obj.n,size(obj.Bd,1)))
8899
assert(size(obj.Bz_x,2) == obj.n || isempty(obj.Bz_x), ...
@@ -92,13 +103,28 @@
92103

93104
% validate given disturbance model
94105
ztest = [obj.Bz_x*zeros(obj.n,1) ; obj.Bz_u*zeros(obj.m,1)];
95-
[muy,vary] = d(ztest);
106+
[muy,vary] = obj.d(ztest);
96107
assert( size(muy,1)==obj.nd, ...
97108
sprintf('Disturbance model d evaluates to a mean value with wrong dimension. Got %d, expected %d',size(muy,1),obj.nd))
98109
assert( all(size(vary)==[obj.nd,obj.nd]), ...
99110
sprintf('Disturbance model d evaluates to a variance value with wrong dimension. Got %d, expected %d',size(vary,1),obj.nd))
100111
end
101112

113+
114+
function zk = z(obj, xk, uk)
115+
%------------------------------------------------------------------
116+
% select variables (xk,uk) -> z
117+
%------------------------------------------------------------------
118+
if ~isempty(obj.Bz_x)
119+
z_xk = obj.Bz_x * xk; else, z_xk=[];
120+
end
121+
if ~isempty(obj.Bz_u)
122+
z_uk = obj.Bz_u * uk; else, z_uk = [];
123+
end
124+
zk = [ z_xk ; z_uk ];
125+
end
126+
127+
102128
function [xkp1, gradx_xkp1] = fd (obj, xk, uk, dt)
103129
%------------------------------------------------------------------
104130
% Discrete time dynamics (ODE1 Euler approximation)
@@ -150,6 +176,7 @@
150176
gradx_xkp1 = eye(obj.n) + dt * gradx_xdot;
151177
end
152178

179+
153180
function [mu_xkp1,var_xkp1] = xkp1 (obj, mu_xk, var_xk, uk, dt)
154181
%------------------------------------------------------------------
155182
% State prediction (motion model) using Extended Kalman Filter
@@ -165,13 +192,7 @@
165192
grad_xkp1 = [gradx_fd; obj.Bd'; obj.Bd'];
166193

167194
% select variables (xk,uk) -> z
168-
if ~isempty(obj.Bz_x)
169-
z_xk = obj.Bz_x * mu_xk; else, z_xk=[];
170-
end
171-
if ~isempty(obj.Bz_u)
172-
z_uk = obj.Bz_u * uk; else, z_uk = [];
173-
end
174-
z = [ z_xk ; z_uk ];
195+
z = obj.z(mu_xk,uk);
175196

176197
% evaluate disturbance
177198
[mu_d, var_d] = obj.d(z);

‎MotionModelGP_InvPendulum_deffect.m

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
%------------------------------------------------------------------
2+
% Programed by:
3+
% - Lucas Rath ([email protected])
4+
% -
5+
% -
6+
%------------------------------------------------------------------
7+
8+
9+
classdef MotionModelGP_InvPendulum_deffect < MotionModelGP_InvPendulum_nominal
10+
%--------------------------------------------------------------------------
11+
% Inverted pendulum dynamics based on the nominal model but with some
12+
% deffect in the actuators
13+
%
14+
% xk+1 = fd_deffect(xk,uk) + Bd * ( d(zk) + w ),
15+
%
16+
% where: zk = [Bz_x*xk ; Bz_u*uk],
17+
% d ~ N(mean_d(zk),var_d(zk))
18+
% w ~ N(0,sigmaw)
19+
%
20+
%
21+
% x = [s, ds, th, dth]' carriage position and pole angle (and derivatives)
22+
% u = [F]' force on the carriage and torque on the pole joint
23+
%
24+
%--------------------------------------------------------------------------
25+
26+
methods
27+
function obj = MotionModelGP_InvPendulum_deffect(Mc, Mp, b, I, l, d, sigmaw)
28+
obj = obj @ MotionModelGP_InvPendulum_nominal(Mc, Mp, b, I, l, d, sigmaw);
29+
end
30+
31+
32+
function xdot = f (obj, x, u)
33+
%------------------------------------------------------------------
34+
% Continuous time dynamics of the inverted pendulum but with some
35+
% deffect (additional disturbance)
36+
% args:
37+
% x: <n,1>
38+
% u: <m,1>
39+
% out:
40+
% xdot: <n,1> time derivative of x given x and u
41+
%------------------------------------------------------------------
42+
% get dynamics from nominal model
43+
xdot = f @ MotionModelGP_InvPendulum_nominal(obj,x,u);
44+
45+
% add deffect
46+
xdot(3) = xdot(3) + (0.1 * x(3) - 0.01*x(4) + deg2rad(3)) *10;
47+
end
48+
49+
% function [xkp1, gradx_xkp1] = fd (obj, xk, uk, dt)
50+
% % get dynamics from nominal model
51+
% [xkp1, gradx_xkp1] = fd @ MotionModelGP_InvPendulum_nominal(obj, xk, uk, dt);
52+
% % deffect
53+
% xkp1(3) = xkp1(3) + (0.1 * xk(3) - 0.01*xk(4) + deg2rad(3)) *1;
54+
% end
55+
56+
end
57+
58+
end

‎MotionModelGP_InvertedPendulum.m renamed to ‎MotionModelGP_InvPendulum_nominal.m

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
%------------------------------------------------------------------
77

88

9-
classdef MotionModelGP_InvertedPendulum < MotionModelGP
9+
classdef MotionModelGP_InvPendulum_nominal < MotionModelGP
1010
%--------------------------------------------------------------------------
1111
% xk+1 = fd(xk,uk) + Bd * ( d(zk) + w ),
1212
%
@@ -49,12 +49,12 @@
4949

5050
methods
5151

52-
function obj = MotionModelGP_InvertedPendulum (Mc, Mp, b, I, l, d, sigmaw)
52+
function obj = MotionModelGP_InvPendulum_nominal (Mc, Mp, b, I, l, d, sigmaw)
5353
%------------------------------------------------------------------
5454
% object constructor
5555
%------------------------------------------------------------------
5656
% call superclass constructor
57-
obj = obj@MotionModelGP(d,sigmaw);
57+
obj = obj @ MotionModelGP(d,sigmaw);
5858
% store parameters
5959
obj.Mc = Mc;
6060
obj.Mp = Mp;

0 commit comments

Comments
 (0)