2
$\begingroup$

I generate a state space model as follows (The details are not important. This part is used to get matrix A_xi and B_xi and the state space model is xi(k+1) = A_xi*xi(k)+B1_xi*u0(k); y(k) = x(k)):

% vehicle dynimcs
h = 0.8; % time headway
N = 4; % number of vehicles 0 leading
tau = 0.2; % actuator lag
Ts = 0.1;% sampling interval
% control gain
kp = 0.45;
kv = 0.84;
ka = 0.5;
% vehicle dynamics
% x_i = [e_i;dv_i;a_i], ei=d_i-h*v_i, dvi = v_i-1-v_i
A1 = [0,1,-h;0 0 -1; 0 0 -1/tau]; % x_i to x_i
A2 = [0 0 0; 0 0 1;0 0 0]; % x_i-1 to x_i
B = [0; 0; 1/tau]; % u_i to x_i
% x0 = [q0;v0;a0]
A0 = [0 1 0;0 0 1; 0 0 -1/tau]; % x0 to x0
B0 = B; % u0 to x0
Ac = zeros(3*N,3*N);
Ac(1:3,1:3) = A0;
Ac(5,3) = 1;
for n = 1:N-1
    Ac(3*n+1:3*n+3,3*n+1:3*n+3) = A1;
    if n < N-1
        Ac(3*n+4:3*n+6,3*n+1:3*n+3) = A2;
    end
end

Bc = zeros(3*N,N);
Bc(1:3,1) = B0;
for n = 1:N-1
    Bc(3*n+1:3*n+3,n+1) = B;
end

veh_dyn_c = ss(Ac,Bc,eye(3*N),zeros(3*N,N));
veh_dyn = c2d(veh_dyn_c,Ts);

    % kalman filter
% x = [d,dv,da]
Ak = [0 1 0; 0 0 1; 0 0 -1/tau];
Ck = [0; 0; -1/tau]; % ui to x known input
Bk = [0; 0; 1/tau]; % ui-1 to x disturbance
H = [1 0 0; 0 1 0];
sysk_c = ss(Ak, [Ck, Bk], H, zeros(2,2));
sysk = c2d(sysk_c,Ts);
delta_d = 0.5; % resolution
delta_v = 0.6;%0.6;%
SNR_db = 20;
SNR = 10^(SNR_db/10);
sigma_d = delta_d*sqrt(3/(2*pi^2*SNR));% standard deviation
sigma_v = delta_v*sqrt(3/(2*pi^2*SNR));% standard deviation (don't forget to square)
var_jerk = 1;%0.15499;% process noise variance (jerk)
[KEST,~,~,L0,~,~] = kalman(sysk,var_jerk, diag([sigma_d^2,sigma_v^2])); % L: innovation gain
L = L0;
    
% whole dynamics
% xi = [x0;x_1;d_1;u_1^-;\hat_{x_1^-};...;x_N-1;d_N-1;u_N-1^-;\hat_{x_N-1^-}]
A_all = zeros(3+8*(N-1),3+8*(N-1)); % xi to xi^+
A_f_all = zeros(3+8*(N-1),3+8*(N-1)); % xi^+ to xi^+
B_all = zeros(3+8*(N-1),1); % u0 to to xi^+
C_all = zeros(3+8*(N-1),2*(N-1)); % epsilon = [epsilon_d1;epsilon_v1;...;epsilon_dN-1;epsilon_vN-1] to xi^+
% x0
A_all(1:3,1:3) = veh_dyn.A(1:3,1:3); % x0 to x0+
B_all(1:3) = veh_dyn.B(1:3,1); % u0 to x0+
% x_i
A_all(4:6,1:3) = veh_dyn.A(4:6,1:3); % x_0 to x_1^+
B_all(4:6) = veh_dyn.B(4:6,1); % u0 to x_1^+
for id1 = 1:N-1
    for id2 = 1:N-1
    A_all(8*id1-4:8*id1-2,8*id2-4:8*id2-2) = veh_dyn.A(3*id1+1:3*id1+3,3*id2+1:3*id2+3); % x_id2 to x_id1^+
    A_f_all(8*id1-4:8*id1-2,8*id2) = veh_dyn.B(3*id1+1:3*id1+3,id2+1); % u_id2 to x_id1^+
    end
end
% u_i
for id = 1:N-1
    A_all(8*id,8*id-4) = kp; % e_i to u_i
    A_all(8*id,8*id-3) = kv; % dv_i to u_i
    A_all(8*id,8*id-2) = ka; % a_i to u_i
    A_f_all(8*id,8*id+3) = ka; % \hat_da_i to u_i
    C_all(8*id,2*id-1) = kp; % epsilon_d_i to u_i
    C_all(8*id,2*id) = kv; % epsilon_v_i to u_i
end
% d_i
for id = 1:N-1
    A_f_all(8*id-1,8*id-4) = 1; % e_i to d_i
    A_f_all(8*id-1,2) = h; % v0 to d_i
    for id2 = 1:id
        A_f_all(8*id-1,8*id2-3) = -h;
    end
end
% \hat{x}_i
for id = 1:N-1
    A_all(8*id+1:8*id+3, 8*id-1) = L(:,1); % d_i to \hat{x}_i
    A_all(8*id+1:8*id+3, 8*id-3) = L(:,2); % dv_i to \hat{x}_i
    C_all(8*id+1:8*id+3, 2*id-1) = L(:,1); % epsilon_d_i to \hat{x}_i
    C_all(8*id+1:8*id+3, 2*id) = L(:,2); % epsilon_v_i to \hat{x}_i
    A_all(8*id+1:8*id+3, 8*id+1:8*id+3) = (eye(3)-L*H)*sysk.A; % \hat{x}_i^- to \hat{x}_i
    A_all(8*id+1:8*id+3, 8*id) = (eye(3)-L*H)*sysk.B(:,1); % u_i^- to \hat{x}_i
end

% feedback
A_xi = (eye(length(A_f_all))-A_f_all)\A_all;
B1_xi = (eye(length(A_f_all))-A_f_all)\B_all; % u0
B2_xi = (eye(length(A_f_all))-A_f_all)\C_all; % epsilon

Then I want to obtain the transfer function between the 13th and 5th output as follows:

[NUM_u,DEN_u] = ss2tf(A_xi,B1_xi,eye(length(A_xi)),zeros(length(A_xi),size(B1_xi,2)),1);
Gamma_v21 = tf(NUM_u(13,:),NUM_u(5,:),Ts); % v2/v1
figure
bode(Gamma_v21)

The result is Bode plot of Gamma_v21

Bode plot

However, when I use a sinusoid signal as u0 and plot these two outputs, the result does not match the Bode plot. For example, when the frequency of u0 is 0.1 rad/s, the magnitude of Bode diagram is -37.4dB. The simulated result is generated with the following code:

L_t = 5000;
t = (1:L_t)*Ts;
u0 = sin(0.1*t);
xi = zeros(3+8*(N-1),L_t);
xi(1:3,1)=[0;20;0];
for id = 1:N-1
    xi(8*id-1,1) = h*xi(2,1);% d_i
    %[id size(xi)]
    xi(8*id+1,1) = xi(8*id-1,1); % hat d_i
    %[id size(xi)]
end
for l_t = 2:L_t
    xi(:,l_t) =  A_xi*xi(:,l_t-1)+B1_xi*u0(l_t-1);
end
figure
hold on
plot(t,xi(5,:))
plot(t,xi(13,:))
legend('5-th output','13-th output')

The result is here.

Result

The ratio of the amplitudes is clearly not -37.4dB. I also tried the frequency 1.11 rad/s (corresponding to the peak of bode amplitude), there is also a mismatch. I wonder what causes this mismatch?

According to @Ben's suggestion, I also tried the transfer function between 5-th output and input u0 as below. The results of Bode plot and simulated frequency input still don't match.

Gamma_v1u0 = tf(NUM_u(5,:),DEN_u,Ts);
figure
bode(Gamma_v1u0)
L_t = 5000;
t = (1:L_t)*Ts;
xi = zeros(3+8*(N-1),L_t);
xi(1:3,1)=[0;20;0];
for id = 1:N-1
    xi(8*id-1,1) = h*xi(2,1);% d_i
    xi(8*id+1,1) = xi(8*id-1,1); % hat d_i
end
u0 = sin(0.1*t);
for l_t = 2:L_t
    xi(:,l_t) =  A_xi*xi(:,l_t-1)+B1_xi*u0(l_t-1);
end
figure
hold on
plot(t,u0)
plot(t,xi(5,:))
legend('u0','5-th output')
title('Frequency 0.1 rad/s')

u0 = sin(1.11*t);
for l_t = 2:L_t
    xi(:,l_t) =  A_xi*xi(:,l_t-1)+B1_xi*u0(l_t-1);
end
figure
hold on
plot(t,u0)
plot(t,xi(5,:))
legend('u0','5-th output')
title('Frequency 1.11 rad/s')
$\endgroup$
9
  • 1
    $\begingroup$ First of all, the Bode plot amplitude corresponds to the steady-state. Maybe wait long enough for the transient to go away and verify what's the amplitude ? $\endgroup$
    – Ben
    Oct 13 at 11:45
  • 1
    $\begingroup$ And usually the transfer function is between an input and an output. For some reason, you used the 13th and 5th output... $\endgroup$
    – Ben
    Oct 13 at 12:02
  • $\begingroup$ @Ben Thank you for your comment. I think the time length is long enough to see the steady state. The second figure shows such a result. $\endgroup$
    – SoftSail
    Oct 13 at 12:03
  • 1
    $\begingroup$ @Ben Thank you for your suggestion. I tried the suggested transfer function, as presented in the edited question, but the results of Bode plot and simulated response still don't match. As for the pole-zero cancellation, how to test it or avoid it? $\endgroup$
    – SoftSail
    Oct 13 at 12:22
  • 1
    $\begingroup$ Check this cds.caltech.edu/~murray/books/AM08/pdf/… You need to check the reachability and obersvability of your SS matrices. $\endgroup$
    – Ben
    Oct 13 at 12:26

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.