Problems and Errors during EKF (extended kalman filter) implementation

15 views (last 30 days)
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;
  2 Comments
Simone Besana
Simone Besana on 16 Aug 2022
Hi, thank you for your answer/comment, I uploaded the file here exported as "previous versions"

Sign in to comment.

Answers (0)

Products


Release

R2022a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!