Problem mit Unscented Kalman Filter in Simulink

WICHTIG: Der Betrieb von goMatlab.de wird privat finanziert fortgesetzt. - Mehr Infos...

Partner:

Forum

Option

[Erweitert]

• Diese Seite per Mail weiterempfehlen

Gehe zu:

Vertunis

Forum-Anfänger


	Beiträge: 11

	Anmeldedatum: 20.01.15

	Wohnort: ---

	Version: ---

Verfasst am: 29.01.2016, 17:52 Titel: Problem mit Unscented Kalman Filter in Simulink

Hiho alle miteinander Smile

Ich habe folgendes Problem und zwar soll ich mithilfe eines UKF ein Volumenstrom beim Kippenvorgang einer gefüllten Flasche abschätzen. Dabei gebe ich mit einer Solltrajektorie den Volumenstrom vor der abgeschätzt werden soll Mein UKF tut bis jetzt folgendes: (Simulation mit ode34)

Ich bekomme dabei diese komischen Überschwinger nicht weg und bin langsam mit meinem Latein am Ende. Hab auch schon Beschränkungen für Sigma Werte versucht hilft alles nix.

Zur Systembeschreibung

Der Volumenstrom ergibt sich nach Toricelli über folgende Gleichung

$q(t) = c \int^{h(t)}_{0} L_f(h_b) \sqrt{2gh_b}dh_b$

mit $L_f$ als Ausgussbreite, $c$ als Flusskonstante und $h$ als Flüssigkeitshöhe über dem Ausgusspunkt der Flasche. Unter der Voraussetzung, dass $L_f$ konstant ist, ergibt sich $q$ wie folgt.

$q(t) =\frac{2}{3}*c*L_f \sqrt{2g} h(t)^{\frac{3}{2}}$

Die Nebenbedingung ist, dass $q$ nie positiv werden darf (physikalisch auch logisch).

$q$ ist Teil der folgenden DGL.

$\frac{dh(t)}{dt} = - \frac{q(h(t))}{A(\theta (t))} - \frac{h(t)}{A(\theta (t))} \frac{\partial A(\theta (t))}{\partial \theta (t)} \omega(t)-\frac{1}{A(\theta (t)} \frac{\partial V_s(\theta (t))}{\partial \theta (t)} \omega(t), $

dabei sind $A(\theta (t)), \frac{\partial A(\theta (t))}{\partial \theta (t)}, \frac{\partial V_s(\theta (t))}{\partial \theta (t)}$ Parameter bezüglich der Flaschegeometrie.

Die Winkelgeschwindigkeit $\omega$ ergibt sich dabei aus der zeitlichen Ableitung des Kippwinkels.

$\frac{d \theta(t)}{dt} = \omega(t),$

Alle anderen Formeln sind erstmal sekundär....

Der Zustandsvektor lautet

$x= \begin{bmatrix} \theta\\ h\\ w_{o}\\ w_{L} \end{bmatrix}$
Unten sind die gebildeten Differenzengleichungen aus den beiden DGL´s (und dem Rest)

$ F (x_k,u_k)= \begin{bmatrix} \displaystyle \theta_k + \Delta T \omega_k\\ \displaystyle h_k + \Delta T (\frac{1}{A(\theta _k)} (-q(h_k)- h_k \frac{\partial A(\theta_k)}{\partial \theta_k} \omega_k- \frac{\partial V_s(\theta_k)}{\partial \theta k} \omega_k)\\ \displaystyle w_{0k}+ \Delta T \rho q(h_k)\\ \displaystyle w_{Lk} - \frac{\Delta T }{T_L}w_{Lk}+ \frac{\Delta T }{T_L}w_{0k} \end{bmatrix}$

UKF Code für Embedded Matlab Function Block in Simulink

Code:

% Copyright 2009 Krisada Sangpetchsong

function [x_dach, P_dach, sqrtP_dach, q] = ukf(omega, zk, x0, P0, Q, R, deltaT, GeoPar, StreckenPar, W, Lambda)

% Input
% omega Winkelgeschwindigkeit
% zk Messungen zk = [Theta; w_L] in [Degr.; kg]
% x0 Initial state estimate [70.5; 0; 0; 0] in [Degr./s ;Degr.; m; kg; kg]
% P0 Initial error covariance matrix
% Q Covariance Matrix of Process
% R Covariance Matrix of Measurement
% deltaT Sample Time
% GeoPar Geometrieparameter [Area,dArea,dVs]
% StreckenPar Parameter der Regelstrecke =[T_t; K_t; g; c; L_f; rho; T_L];
% W = Weights [Wm Wc]

% Output
% x_dach Zustandsabschätzung "a posteriori"
% P_dach Fehlerkovarianzmatrix "a posteriori"
% sqrtP_dach Square root of the diagonal term of the estimation error covariance matrix
% q_dach Abgeschätzer Ausfluss

%% Declare persistent parameters

persistent x % Zustandsvektor x = [Theta; w_L] in [Degr./s ;Degr.; m; kg; kg]
persistent P % Error Kovarianzmatrix
persistent Qk % Kovarianzmatrix des Prozessrauschens
persistent Rk % Kovarianzmatrix des Messrauschens
persistent Xm % Temporärer Zwischenspeicher für "predicted" state Sigma Punkte
persistent Z % Temporärer Zwischenspeicher für "predicted" measurement Sigma Punkte

% Initialise persistent parameters

if isempty(x)
x = x0;
end

if isempty(P)
P = P0;
end

if isempty(Qk)
Qk = Q;
end

if isempty(Rk)
Rk = R;
end

if isempty(Xm)

Xm = zeros(size(x0,1),size(W,1));
end

if isempty(Z)

Z = zeros(size(zk,1),size(W,1));

end

%% Start UKF Routine

% Bestimmung der Sigma Punkte X
L = size(x,1);
O = zeros(size(x,1),1);
S = chol((L+Lambda)*P)'; % S = chol(R) mit S'*S = R (Cholesky-Zerlegung)

X = [O S -S]+ repmat(x,1,L*2+1);

% Time Update: Sigma Punkte durchlaufen nichtlineare Funktion F

%Parameter für die Regelstrecke
g =StreckenPar(3); %Erdbeschleunigung [m/s²]
c =StreckenPar(4); % 0<c<1 Flussratenkoeffizient
L_fconst=StreckenPar(5); %Geometrie-Parameter der Gießschnauze [m]
rho=StreckenPar(6);
T_L=StreckenPar(7); %[s]

L2 = size(X,2); % Durchlauf der Sigmawerte (Spaltenweise)
for k = 1:L2

% Systemgleichungen

if X(2,k)<0
q = -2/3*abs(X(2,k))^(3/2)*c*L_fconst*(2*g)^(1/2);
else
q = 2/3*(X(2,k))^(3/2)*c*L_fconst*((2*g)^(1/2)); % Signumfunktion bei q beachten muss immer Betrag -> |h(t)| sein
end

x1 = X(1,k)+deltaT*omega;
x2 = X(2,k)+deltaT*((1/GeoPar(1))*(-q-X(2,k)*GeoPar(2)*omega-GeoPar(3)*omega));
x3 = X(3,k)+deltaT*(rho*q);
x4 = X(4,k)+deltaT*(X(4,k)*(-1/T_L)+1/T_L*X(3,k));

Xm(:,k) = [x1; x2; x3; x4];

end

% Find the prediction mean (xm) and covariance matrix (Pm)

[xm, Pm] = ukf_mean_covariance(Xm, W);

Pm = Pm + Qk;

% Predicted measurements sigma points
for k = 1:L2
Z(:,k) = H(Xm(:,k)); % must be modified to match your model (only if the h function has more input or output parameters).
end

% Find mean and covariance (Pz=Pzz (2x2))
[z, Pz] = ukf_mean_covariance(Z, W);

Pz = Pz + Rk;

% Cross covariance matrix
Pxz = ukf_covariance(Xm, Z, W);

% Kalman gain matrix and state estimation
Kk = Pxz/Pz;
x = xm + Kk*(zk - z); % Find the state estimates
P = Pm - Kk*Pz*Kk'; % Estimation error covariance matrix
P = 0.5*(P+P'); % Prevent numerical problem

% End UKF-Routine

%% Ausgänge
x_dach = x;
P_dach = P;
sqrtP_dach = sqrt(diag(P));

%% This is the nonlinear measurement model
% H(x) must be modified to match your measurement model.

function z = H(x)

z1 = x(1);
z2 = x(4);
z = [z1; z2];

%% This function computes the mean and covariance matrix from the sigma points.
% m is the mean computed from the sigma points.
% P is the covariance matrix computed from the sigma points and the mean.
function [m, P] = ukf_mean_covariance(sigmaX, W)
L2 = size(sigmaX,2); % L2=13 (Entspricht Anzahl der Gewichtungen W)

sigmaXW=zeros(size(sigmaX)); % Erstellung Vektor für Weighted Sigmas

for k=1:L2
sigmaXW(:,k)= W(k,1)*sigmaX(:,k);
end

m = sum(sigmaXW,2); % sum(A,dim) returns the sum along dimension dim. For example, if A is a matrix, then sum(A,2) is a column vector containing the sum of each row.

Pxx = zeros(size(sigmaX,1)); % size L (5x11-Vektor)
for j = 1:L2
Pxx = Pxx + W(j,2)*((sigmaX(:,j)-m)*(sigmaX(:,j)-m)'); %summiert zu jedem vorherigen P die kte-Spalte mal die k-te-Spalte Transponiert.

end
P=Pxx;

%% This function computes the cross covariance matrix from two sets of sigma points.
% P is the cross covariance matrix computed from the sigma points and the mean.
% sigmaX and sigmaY are the sigma points of X and Y respectively.
function P = ukf_covariance(sigmaX, sigmaY, W)

L2 = size(sigmaX,2); % 2*L=13

sigmaXW=zeros(size(sigmaX));
for k = 1:L2
sigmaXW(:,k) = W(k,1)*sigmaX(:,k); % Multiplikation mit Gewichtung W(m)
% Durchlauf Spalte für Spalte
end

mx = sum(sigmaXW,2);

L2=size(sigmaY,2);
sigmaYW=zeros(size(sigmaY));

for i = 1:L2
sigmaYW(:,i) = W(i,1)*sigmaY(:,i); % Multiplikation mit Gewichtung W(m)
% Durchlauf Spalte für Spalte
end

my = sum(sigmaYW,2);

Pxy = zeros(size(sigmaX,1),size(sigmaY,1)); % size 5x2

for k = 1:L2
Pxy = Pxy + W(k,2)*((sigmaX(:,k)-mx)*(sigmaY(:,k)-my)');
end

P=Pxy;

Funktion ohne Link?

Ich vermute, dass das was mit der gleich beschriebenen Formel für den Volumenstrom $q$ bzw änderung der Füssigkeitshöhe $\frac{dh(t)}{dt}$ zu tun hat. Möglichweise hat das auch iwas mit der Winkelgeschwindigkeit zu tun.

Wäre super wenn mir jemand irgendeinen Tipp geben kann. Laughing

20_12_15_UKF.zip

Beschreibung:

Hier das Modell zum selber anschaun mit allen Daten im Matlab Code : Anfangswerte Kovarianzmatrix etc.
Der Matlab-Code führt das Simulink Modell selber aus und Plottet die Daten

Download

Dateiname:

20_12_15_UKF.zip

Dateigröße:

126.32 KB

Heruntergeladen:

560 mal

Vertunis

Themenstarter

Forum-Anfänger


	Beiträge: 11

	Anmeldedatum: 20.01.15

	Wohnort: ---

	Version: ---

Verfasst am: 30.01.2016, 00:42 Titel:

Edit: nochmal der Link zu dem Bild

http://www.bilder-upload.eu/show.php?file=6098f6-1454083084.png

Vertunis

Themenstarter

Forum-Anfänger


	Beiträge: 11

	Anmeldedatum: 20.01.15

	Wohnort: ---

	Version: ---

Verfasst am: 01.02.2016, 22:19 Titel:

Wenn ich als Solver ode1 anstatt ode34 verwende, funktioniert der UKF lustigerweise wie er sollte.

http://www.bilder-upload.eu/show.php?file=66bc1b-1454358724.png

Kann dazu vllt jemand was sagen wieso das so ist ? Ich dachte der solver hätte keinen einfluss darauf ?

Heber

Forum-Fortgeschrittener


	Beiträge: 64

	Anmeldedatum: 01.02.17

	Wohnort: ---

	Version: R2013a

Verfasst am: 14.02.2017, 09:56 Titel:

Hallo, hattest du das UKF damals als M-Function in einem Simulink Modell?

Vertunis

Themenstarter

Forum-Anfänger


	Beiträge: 11

	Anmeldedatum: 20.01.15

	Wohnort: ---

	Version: ---

Verfasst am: 14.02.2017, 11:54 Titel:

Hallo Heber,

ja ich habe da ein Matlab Function Block mit Persistent Variablen verwendet, die Persistent Variablen haben aber einen Fehler verursacht, daher habe ich mit Decouplern weitergearbeitet, um die Zustände zwischenzuspeichern.

Heber

Forum-Fortgeschrittener


	Beiträge: 64

	Anmeldedatum: 01.02.17

	Wohnort: ---

	Version: R2013a

Verfasst am: 14.02.2017, 13:28 Titel:

Hallo Vertunis,

hast du die alten Daten noch?

Ich habe zur Zeit auch ein Problem, allerdings mit symbolischen Variablen in der function. Evtl könnte dein Ansatz besser geeignet sein. Ich habe die auf instrisic gestellt, aber das war nicht erfolgreich.

Bei mir geht es darum ein EKF zu entwickeln.

Vertunis

Themenstarter

Forum-Anfänger


	Beiträge: 11

	Anmeldedatum: 20.01.15

	Wohnort: ---

	Version: ---

Verfasst am: 14.02.2017, 14:24 Titel:

Mit dem EKF habe ich leider nie gearbeitet. Prinzipiell ist der UKF leichter aufzubauen als der EKF und bietet außerdem eine höhere Approximationsgenauikgeit soweit ich mich erinnere. Was für Probleme machen die symbolischen Variablen denn ?

Heber

Forum-Fortgeschrittener


	Beiträge: 64

	Anmeldedatum: 01.02.17

	Wohnort: ---

	Version: R2013a

Verfasst am: 14.02.2017, 15:16 Titel:

Ich dachte das EKF sein einfacher... Naja ist egal. Es bedarf auf jeden Fall weniger Rechenzeit.

Ich hatte zunächst versucht meine nicht-lineare Gleichung symbolisch darzustellen. Dann müssen für das EKF die Jacobimatrizen bestimmen. Ich habe dann den Subs Befehl benutzt um in den Gleichungen die Symbolischen Variablen durch die aktuellen Zustandsvektoren zu ersetzen.

Aber bisher habe ich kein zufriedenstellendes Ergebniss.

Im Grunde benötigt man für das EKF nur seine beiden Funktionen f und g aus der folgenden Vorschrift
x(k+1) = f(x(k),u(k)) + w(k)
y(k) = g(x(k),u(k)) + v(k)

Gegeben habe ich die Funktion f und g in kontinuierlicher Form. Es muss also eine Diskrete Form hier von bestimmt werden und dann müssen die linearisierten Matrizen A, B, C bestimmt werden. A ist die Einheitsmatrix plus eine Abtastzeit* die Jacobimatrix df/dx. B entspricht df/du und C entspricht dg/dx.

Hat man diese Matrizen ergeben sich die übrigen Berechnungen wie beim gewöhnlichen Kalman Filter.

Vertunis

Themenstarter

Forum-Anfänger


	Beiträge: 11

	Anmeldedatum: 20.01.15

	Wohnort: ---

	Version: ---

Verfasst am: 14.02.2017, 18:35 Titel:

Okay, wie gesagt habe ich noch nie mit dem EKF gearbeitet. Der UKF hat den Vorteil dass du keine Jacobi Matrizen brauchst und Linearisieren musst du auch nicht. Versuch mal den UKF aufzubauen. Wichtig dabei sind die Parameterwahl (alpha kappa beta) und die Verwendung von Constraint Conditions (https://pdfs.semanticscholar.org/8e.....70d309d57973c61421760.pdf) falls deine Funtkion physikalische Grenzen besitzt wie z.b. nur positiver Ausfluss von Flüssigkeit. Und anstatt mit "Persitent Variablen" zu arbeiten, kannst du wie gesagt Decoupler verwenden die deine Zustände aus dem Funktion-Block führen zwischenspeichern und wieder einspeisen.

Heber

Forum-Fortgeschrittener


	Beiträge: 64

	Anmeldedatum: 01.02.17

	Wohnort: ---

	Version: R2013a

Verfasst am: 15.02.2017, 11:14 Titel:

Hallo Vertunis, könntest du vllt. die grundsätzliche Vorgehensweise zur Entwicklung eines UKFs erläutern?

Wie wähle ich die Parameter Alpha, Beta und Gamma und wie berücksichtige ich Beschränkungen?

Danke!

Vertunis

Themenstarter

Forum-Anfänger


	Beiträge: 11

	Anmeldedatum: 20.01.15

	Wohnort: ---

	Version: ---

Verfasst am: 15.02.2017, 13:51 Titel:

Die pfd zur Beschränkung habe ich schon als Link gepostet. Da beschränkst du deine Zustände im UKF-Algorithmus entsprechend gewissen Kriterien.

Vorgehensweise beim UKF:
- Diskretisieren der Systemgleichungen
- Aufbau des UKF über die entsprechenden Gleichungen im Matalb Function Block mithilfe deiner diskretisierten Systemgleichungen.
z.b.: nach http://citeseerx.ist.psu.edu/viewdo.....amp;rep=rep1&type=pdf

UKF Eingang:
-Gewichtungen der einzelnen Zustände $W$ ,
-Anfangsbedingungen deiner Zustände (entsprechen Mittelwert ${x}$ (Vektor entsprechend Systemgleichungen))
- Kovarianzmatrix $P_{xx}^{(+)}$

- Eingangssignal $u$

-Messwerte $z$

- Kovarianzmatrix System(Q) und Messrauschen (R)

UKF Algorithmus (rekusiv):

-Bestimmung der Sigmapunkte $X$ (L x 2L+1 -Matrix) welche Statistische Eingenschaften der einzelnen Zustände besitzen (mit Mittelwert x , Kovarianzmatrix P, Parameter Lambda (aus alpha beta kappa) -> L sind dabei die Anzahl der Zustände

- Durchlauf der Sigmapunkte durch deine Nichtlineare Funktion $F(X)$

- Bestimmung von Kovarianzmatrix $P_{xx}^{(-)}$ (das "-" bedeutet vor korrektur und das "+" nach Korrektur)

- Bestimmung von Kovarianzmatrix $P_{yy}^{(-)}$

- Bestimmung des Kalman Gain aus $P_{xx}^{(-)}$ und $P_{yy}^{(-)}$

- Korrektur deiner alten Zustände $x^{(-)}$ und Kovarianzpartrix $P_{xx}^{(-)}$ mit Kalman Gain -> ergibt $P_{xx}^{(+)}$

- Ausgang: korrigiertes $x^{(+)}$ und $P_{xx}^{(+)}$

und das ganze für jeden Iterationsschritt neu

Beispiel mit dem ich Anfangs gearbeitet habe:
[url]
https://de.mathworks.com/matlabcent.....ed-kalman-filtering[/url]

Heber

Forum-Fortgeschrittener


	Beiträge: 64

	Anmeldedatum: 01.02.17

	Wohnort: ---

	Version: R2013a

Verfasst am: 15.02.2017, 16:44 Titel:

Wie bekomme ich denn die Diskretisierung der Zustandsgleichung hin?

Im linearen Fall mit den Matrizen A.B,C weiß ich wie das geht aber wiei st das im nichtlinearen Fall?

Vertunis

Themenstarter

Forum-Anfänger


	Beiträge: 11

	Anmeldedatum: 20.01.15

	Wohnort: ---

	Version: ---

Verfasst am: 15.02.2017, 18:05 Titel:

schau mal im Internet nach Euler-Vorwärtsdiskretisierung

Heber

Forum-Fortgeschrittener


	Beiträge: 64

	Anmeldedatum: 01.02.17

	Wohnort: ---

	Version: R2013a

Verfasst am: 01.03.2017, 14:04 Titel:

Wie groß sollte die Zeitkonstante des Filters sein?

Gleich der Abtastfrequenz, mit der die Messsignale eingelesen werden?

Heber

Forum-Fortgeschrittener


	Beiträge: 64

	Anmeldedatum: 01.02.17

	Wohnort: ---

	Version: R2013a

Verfasst am: 02.03.2017, 12:36 Titel:

Kann es sein, dass zwei mal 'end' fehlt in deinem code?


Einstellungen und Berechtigungen
Beiträge der letzten Zeit anzeigen:	Du kannst Beiträge in dieses Forum schreiben. Du kannst auf Beiträge in diesem Forum antworten. Du kannst deine Beiträge in diesem Forum nicht bearbeiten. Du kannst deine Beiträge in diesem Forum nicht löschen. Du kannst an Umfragen in diesem Forum nicht mitmachen. Du kannst Dateien in diesem Forum posten Du kannst Dateien in diesem Forum herunterladen

Impressum | Nutzungsbedingungen | Datenschutz | FAQ |

RSS

Hosted by:

Copyright © 2007 - 2024 goMatlab.de | Dies ist keine offizielle Website der Firma The Mathworks

MATLAB, Simulink, Stateflow, Handle Graphics, Real-Time Workshop, SimBiology, SimHydraulics, SimEvents, and xPC TargetBox are registered trademarks and The MathWorks, the L-shaped membrane logo, and Embedded MATLAB are trademarks of The MathWorks, Inc.