Problems and Errors during EKF (extended kalman filter) implementation

Hi, I am quite new to matlab and simulink. I am trying to implement an EKF to estimate the states of my system and I am trying to do this in simulink. Mine is an easy nonlinear system. I have two unicycles moving in a plane, they can measure their relative distance and i am trying to estimate x1, y1, theta1, x2, y2 and theta2 starting from this measure and the velocity of both (velocity is known and it is the input of the system). This is the first time that i try to do this implementation, so it's probably i made some errors that i can't see.

I put the simulink file in "Attachments" and the code about nonlinear system, prediction and correction of EKF in the end of the message. Sorry, but the comment are in italian, if you want i can translate them.

This simulink file gives me this error:

"=== Simulation (Elapsed: 0.394 sec) ===

Warning:'Output Port 3' of 'Uniciclo/Demux' is not connected.

Error:Cannot solve algebraic loop involving 'Uniciclo/MATLAB Function2' because it consists of blocks that cannot be assigned algebraic variables, such as blocks with discrete-valued outputs, blocks with non-double or complex outputs, Stateflow blocks, or nonvirtual subsystems. Consider breaking the algebraic loop. For example, add a delay or a memory block to the loop. To see more details about the loops use the command Simulink.BlockDiagram.getAlgebraicLoops(bdroot)

Error:Algebraic loop error with 'Uniciclo/MATLAB Function1'

Error:Algebraic loop error with 'Uniciclo/MATLAB Function2'

Thank you all for reading and for your answer.

Dynamic system code:

function [dq1,dq2,z_meas]= Uniciclo1(q1,q2,u1,u2)

x1 = q1(1);

y1 = q1(2);

theta1 = q1(3);

x2 = q2(1);

y2 = q2(2);

theta2 = q2(3);

v1 = u1(1);

omega = u1(2);

v2 = u2(1);

w = u2(2);

% cinematica uniciclo 1

dx1 = v1*cos(theta1);

dy1 = v1*sin(theta1);

dtheta1 = omega;

% cinematica uniciclo 2

dx2 = v2*cos(theta2);

dy2 = v2*sin(theta2);

dtheta2 = w;

% calcolo uscite sistema (distanza fra unicicli)

z = sqrt((x2-x1)^2 + (y2-y1)^2);

% scrivo uscite della matlab function

dq1 = [dx1 dy1 dtheta1]';

dq2 = [dx2 dy2 dtheta2]';

z_meas = z;

The code abou the prediction (=predizione) part of EKF

function [q_post_pre,z_hat_out,P_out] = predizione(q_pre_pre,u,P)

% Questa matlab function implementa il filtro di kalman esteso usato per il

% tracciamento di due unicicli dei quali è disponibile la distanza (2

% rispetto a 1)

%

% Gli stati del processo sono dati da:

% stato = [x1 y1 theta1 x2 y2 theta2];

%

% e la misurazione è data da:

% z = sqrt( (x2-x1)^2 + (y2-y1)^2 );

if isempty(P)

%faccio un'inizializzazione coi valori iniziali

q_pre_pre = [0 -1 0 0 -2 0];

P = eye(6);

end

% Calcolo l'evoluzione dello stato linearizzata nel punto stimato

F = [0 0 -u(1)*sin(q_pre_pre(3)) 0 0 0; 0 0 cos(q_pre_pre(3))*u(1) 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 -u(3)*sin(q_pre_pre(6)); 0 0 0 0 0 cos(q_pre_pre(6))*u(3); 0 0 0 0 0 0];

% Calcolo lo step di PREDIZIONE

q_post_pre = [cos(q_pre_pre(3))*u(1), sin(q_pre_pre(3))*u(1), u(2), cos(q_pre_pre(6))*u(3), sin(q_pre_pre(6))*u(3), u(4)];

P_out = F*P*F';

z_hat_out = sqrt((q_pre_pre(4)-q_pre_pre(1))^2 + (q_pre_pre(5)-q_pre_pre(2))^2);

The code about the correction (=correzione) part of the EKF:

function [z_hat_out,q_post_post,P_out] = Correzione(q_post_pre,z_meas,z_hat,P)

R=1;

% Assegno alle variabili dello stato i valori corretti di q_hat

%x1 = q_post_pre(1);

%y1 = q_post_pre(2);

%theta1 = q_post_pre(3);

%x2 = q_post_pre(4);

%y2 = q_post_pre(5);

%theta2 = q_post_pre(6);

% Scrivo il linearizzato delle uscite

H = [(2*q_post_pre(1) - 2*q_post_pre(4))/(2*((q_post_pre(1) - q_post_pre(4))^2 + (q_post_pre(2) - q_post_pre(5))^2)^(1/2)), (2*q_post_pre(2) - 2*q_post_pre(5))/(2*((q_post_pre(1) - q_post_pre(4))^2 + (q_post_pre(2) - q_post_pre(5))^2)^(1/2)), 0, -(2*q_post_pre(1) - 2*q_post_pre(4))/(2*((q_post_pre(1) - q_post_pre(4))^2 + (q_post_pre(2) - q_post_pre(5))^2)^(1/2)), -(2*q_post_pre(2) - 2*q_post_pre(5))/(2*((q_post_pre(1) - q_post_pre(4))^2 + (q_post_pre(2) - q_post_pre(5))^2)^(1/2)), 0];

% Calcolo il guadagno del filtro di Kalman

S = H*P*H' + R;

K = P*(H')*(S^(-1));

% Calcolo la CORREZIONE

q_post_post = q_post_pre + (K*(z_meas-z_hat))';

P_out = P + K*S*(K');

% Scrivo il non lineare delle uscite

h = sqrt((q_post_post(4)-q_post_post(1))^2 + (q_post_post(5)-q_post_post(2))^2);

z_hat_out = h;

[Benjamin Thompson]

Can you post a Simulink file for R2021B? I cannot updgrade to R2022A yet.

[Simone Besana]

Hi, thank you for your answer/comment, I uploaded the file here exported as "previous versions"

Answer 1

[Yash]

Hi Simone,

I understand that you are facing issues with algebraic loops in your Simulink model. Algebraic loops occur when there is a feedback loop in the model that creates a dependency between input and output signals at the same time step. This can lead to convergence problems and simulation errors.

You can refer to the below MATLAB Answer to know more about algebraic loops:

https://www.mathworks.com/matlabcentral/answers/95310-what-are-algebraic-loops-in-simulink-and-how-do-i-solve-them

Your model contains two algebraic loops as shown in the below picture:

To break the algebraic loop, you can introduce a memory block (or a Unit Delay block) in the loop. In your case, you can use a memory block to break the loop as shown below:

However, after removing the algebraic loop, the value of "H" is reaching NaN in the "Correzione" function, and hence you will get errors. To resolve this, you can replace NaN values with zeros or ones as shown below:

H = [(2*q_post_pre(1) - 2*q_post_pre(4))/   % your code
H(isnan(H)) = ones(1,nnz(isnan(H)));        % add this line

I hope this helps you to resolve the issue.

Best Regards,

Yash