Fix code errors,

syms u v w q r theta psi Xprop delta_s delta_r

m = 359000; %massa
Xuu   = -1.62;    % Axial Drag
Xwq   = -3.55e1;  % Added mass cross-term
Xqq   = -1.93;    % Added mass cross-term
Xvr   =  3.55e1;  % Added mass cross-term
Xrr   = -1.93;    % Added mass cross-term
Yvv   = -1.31e3;  % Cross-flow drag
Yrr   =  6.32e-1; % Cross-flow drag
Yuv   = -2.86e1;  % Body lift force and fin lift
Yur   =  5.22;    % Added mass cross-term and fin lift
Zww   = -1.31e2;  % Cross-flow drag 
Zqq   = -6.32e-1; % Cross-flow drag
Zuw   = -2.86e1;  % Body lift force and fin lift
Zuq   = -5.22;    % Added mass cross-term and fin lift

% Center of Gravity wrt Origin at CB
xg = 0;
yg = 0;
zg = 1.96e-2;

% Control Fin Coefficients
Yuudr =  9.64;
Nuudr = -6.15;
Zuuds = -9.64; % Fin Lift Force

% Cross flow drag and added mass terms
Mww   =  3.18; % Cross-flow drag
Mqq   = -1.88e2; % Cross-flow drag
Muq   = -2; % Added mass cross term and fin lift
Muw   =  2.40e1; % Body and fin lift and munk moment
Mwdot = -1.93; % Added mass
Muuds = -6.15; % Fin lift moment
Nvv   = -3.18; % Cross-flow drag
Nrr   = -9.40e1; % Cross-flow drag
Nuv   = -2.40e1; % Body and fin lift and munk moment

% Moments of Inertia wrt Origin at CB
Ixx  = 1.77e-1;   
Iyy  = 3.45;  
Izz  = 3.45;

Nur = -2.00; % Added mass cross term and fin lift

% Non-linear Moments Coefficients
Xudot = -9.30e-1; % Added mass
Yvdot = -3.55e1;  % Added mass
Nvdot =  1.93;    % Added mass
Mwdot = -1.93;    % Added mass
Mqdot = -4.88;    % Added mass
Zqdot = -1.93;    % Added mass
Zwdot = -3.55e1;  % Added mass
Yrdot =  1.93;    % Added mass
Nrdot = -4.88;    % Added mass

% Accelerations Matrix (Prestero Thesis page 46)
Amat = [(m - Xudot) 0              0               m*zg            -m*yg          0 0;
    0           (m - Yvdot)    0               0               (m*xg - Yrdot) 0 0;
    0           0              (m - Zwdot)     (-m*xg - Zqdot) 0              0 0;
    m*zg        0              (-m*xg - Mwdot) (Iyy - Mqdot)   0              0 0;  
    -m*yg       (m*xg - Nvdot) 0               0               (Izz - Nrdot)  0 0;
    0           0              0               0               0              1 0;
    0           0              0               0               0              0 1];

% Inverse Mass Matrix
Minv = inv(Amat);

var = [u v w q r theta psi];
a = [Xuu*u*abs(u)+(Xwq-m)*w*q+(Xqq + m*xg)*q^2+(Xvr+m)*v*r+(Xrr + m*xg)*r^2;
    Yvv*v*abs(v)+Yrr*r*abs(r)+Yuv*u*v+(Yur-m)*u*r-(m*zg)*q*r;
    Zww*w*abs(w)+Zqq*q*abs(q)+Zuw*u*w+(Zuq+m)*u*q+(m*zg)*q^2;
    Mww*w*abs(w)+Mqq*q*abs(q)+(m*zg)*v*r-(m*zg)*w*q+(Muq-m*xg)*u*q+Muw*u*w+delta_s;
    Nvv*v*abs(v)+Nrr*r*abs(r)+Nuv*u*v+(Nur+m*xg)*u*r+delta_r;
    q;
    r/cos(theta)];
Ja = jacobian(a, var);
T = [1 1 1 1 1 0 0];
j = subs(Ja, var, T);
a1 = double(subs(j,{u,v,w,q,r,theta,psi},{1,1,1,1,1,1,1}));
A11 = Minv*a1;

varb = [Xprop delta_r delta_s];
b = [Xuu*u*abs(u)+(Xwq-m)*w*q+(Xqq + m*xg)*q^2+(Xvr+m)*v*r+(Xrr + m*xg)*r^2+Xprop;
    Yvv*v*abs(v)+Yrr*r*abs(r)+Yuv*u*v+(Yur-m)*u*r-(m*zg)*q*r+Yuudr*u^2*delta_r;
    Zww*w*abs(w)+Zqq*q*abs(q)+Zuw*u*w+(Zuq+m)*u*q+(m*zg)*q^2+Zuuds*u^2*delta_s;
    Mww*w*abs(w)+Mqq*q*abs(q)+(m*zg)*v*r-(m*zg)*w*q+(Muq-m*xg)*u*q+Muw*u*w+Muuds*u^2*delta_s;
    Nvv*v*abs(v)+Nrr*r*abs(r)+Nuv*u*v+(Nur+m*xg)*u*r+Nuudr*u^2*delta_r;
    q;
    r/cos(theta)];
bb = jacobian(b, varb);
b1 = double(subs(bb,{u,Xprop,delta_r,delta_s},{1,1,1,1}));
B11 = Minv * b1;

Ac = A11;
Bc = B11;

% Check controllability
co = rank(ctrb(Ac, Bc));
disp(['Controllability Rank: ', num2str(co)]);

% Discretize the system
Ts = 0.01; % sample time
[Ad, Bd] = c2d(Ac, Bc, Ts);

Cd=eye(7); %output matrices
Dd=zeros(7,3);
% Check observability
ob = rank(obsv(Ac,Cd)); 
disp(['Observability Rank: ', num2str(co)]);

%%
% Define sample time
Ts = 0.01;

% Simulate the system to generate state data
% Define time span for simulation
T = 0:Ts:10; % 10 seconds of simulation

% Initial state
x0 = [0; 0; 0; 0; 0; 0; 0];

% Define system matrices from previous code
A = Ac;
B = Bc;
C = Cd;
D = Dd;

% Create a state-space system
sys = ss(A, B, C, D, Ts);

% Define input (for simplicity, consider zero input or some control inputs)
U = zeros(length(T), size(B, 2));

% Simulate the system
[Y, T, X_true] = lsim(sys, U, T, x0);

% Add measurement noise
noise_std = 0.01; % standard deviation of noise
Y_noisy = Y + noise_std*randn(size(Y));

% Assuming reference data for X, Y, Z
X_ref = out.xpos(100:200)'; % Replace with actual reference data
Y_ref = out.ypos(100:200)'; % Replace with actual reference data
Z_ref = out.zpos(100:200)'; % Replace with actual reference data

% Implement Moving Horizon Estimator (MHE)
N = 10; % Horizon length
x_hat = zeros(length(T), length(x0)); % State estimates
x_hat(1, :) = x0'; % Initial state estimate

% Define the weight matrices for the cost function
Q = eye(length(x0)); % State weights
R = eye(3); % Measurement weights

for k = N+1:length(T)
    % Define optimization problem for MHE
    H = [];
    f = [];
    for j = max(1, k-N):k
        % State error term
        H = blkdiag(H, Q);
        f = [f; -Q * x_hat(j, :)'];
    end
    for j = max(1, k-N):k
        % Measurement error term
        idx = min(j, length(X_ref)); % Ensure idx is within bounds
        measurement_error = [X_ref(idx) - x_hat(j, 1); Y_ref(idx) - x_hat(j, 2); Z_ref(idx) - x_hat(j, 3)];
        H = blkdiag(H, R);
        f = [f; -R * measurement_error];
    end
    
    % Solve the optimization problem (minimize the cost function)
    % Use quadprog for quadratic programming
    options = optimoptions('quadprog', 'Display', 'off');
    x_est = quadprog(H, f, [], [], [], [], [], [], [], options);
    
    % Take the last estimated state as the current state estimate
    x_hat(k, :) = x_est(end-length(x0)+1:end)';
end

% Compute 3D trajectory based on state estimates
X = zeros(length(T), 1);
Y = zeros(length(T), 1);
Z = zeros(length(T), 1);

for k = 1:length(T)
    u = x_hat(k, 1);
    v = x_hat(k, 2);
    w = x_hat(k, 3);
    theta = x_hat(k, 6);
    psi = x_hat(k, 7);
    phi = 0; % given phi = 0
    
    X(k) = u*cos(psi)cos(theta) + v(cos(psi)sin(phi)*sin(theta)-sin(psi)*cos(phi)) + w(sin(phi)*sin(psi)+cos(psi)*cos(phi)*sin(theta));

Invalid expression. Check for missing multiplication operator, missing or unbalanced delimiters, or other syntax error. To construct matrices, use brackets instead of parentheses.

    Y(k) = u*sin(psi)cos(theta) + v(sin(psi)sin(phi)*sin(theta)+cos(psi)*cos(phi)) + w(sin(psi)*cos(phi)*sin(theta)-cos(psi)*sin(phi));
    Z(k) = -u*sin(theta) + v*sin(phi)*cos(theta) + w*cos(phi)*cos(theta);
end

% Display the estimated states
disp('Estimated states over the horizon:');
disp(x_hat);
% % Plot the 3D trajectory
% figure;
% plot3(X, Y, Z, 'LineWidth', 1.5);
% grid on;
% xlabel('X');
% ylabel('Y');
% zlabel('Z');
% title('3D Trajectory of AUV');
% legend('Estimated Trajectory');

[Ayush Aniket]

Please specify the error you are encountering. Refer this: https://www.mathworks.com/matlabcentral/answers/6200-tutorial-how-to-ask-a-question-on-answers-and-get-a-fast-answer

[Rik]

You didn't bother to use proper formatting, to reduce your problem down to the smallest size that would reproduce the issue, or to ask an explicit question.

Why exactly should anyone bother to help you?

Answer 1

[Steven Lord]

The error message is correct.

Invalid expression. Check for missing multiplication operator, missing or unbalanced delimiters, or other syntax error. To construct matrices, use brackets instead of parentheses. (Emphasis added.)

You're missing a few multiplication operators (the * symbol) between some of the expressions.

X(k) = u*cos(psi)cos(theta) + ...
    v(cos(psi)sin(phi)*sin(theta)-sin(psi)*cos(phi)) + ...
    w(sin(phi)*sin(psi)+cos(psi)*cos(phi)*sin(theta));

cos(phi)cos(theta) in the first term should be cos(phi)*cos(theta). Your second term is also missing one; do you see it?

Check for more missing multiplication operators in the rest of your code as well.