Mein MATLAB Forum - goMatlab.de

Mein MATLAB Forum

 
Gast > Registrieren       Autologin?   
Bücher:

Studierende:
Praktikant Toolentwicklung Matlab (m/w)
Branche: Beratung, Expertise, Fahrzeugtechnik, Fahrzeugteile, Technische Dienstleistungen
MBtech Group GmbH & Co. KGaA - Fellbach

Abschlussarbeit / Praktikum: Entwicklung Matlab (m/w)
Branche: Informationstechnologie, Elektrotechnik, Elektronik
GIGATRONIK Technologies GmbH - Ulm

Studentischer Mitarbeiter (m/w) - DevOps / Toolentwicklung
Branche: mehrere
IAV GmbH - Ingenieurgesellschaft Auto und Verkehr - Berlin

Praktikant (m/w) Bereich Konzernvorausentwicklung - Aktoren sicherheitsrelevanter Anwendungen
Branche: Fahrzeugtechnik, Fahrzeugteile, Maschinentechnik, Industrietechnik
MAHLE International GmbH - Stuttgart

Praktikum - Softwareentwicklung Java
Branche: mehrere
IAV GmbH - Ingenieurgesellschaft Auto und Verkehr - Dresden, Gifhorn

weitere Angebote

Partner:


Vermarktungspartner


Forum
      Option
[Erweitert]
  • Diese Seite per Mail weiterempfehlen
     


Gehe zu:  
Neues Thema eröffnen Neue Antwort erstellen

Parameterschätzung mit nichtlinearem Kalman-Filter (EKF/UKF)

 

EliteTUM
Forum-Fortgeschrittener

Forum-Fortgeschrittener


Beiträge: 70
Anmeldedatum: 21.04.11
Wohnort: München
Version: ---
     Beitrag Verfasst am: 05.01.2012, 19:56     Titel: Parameterschätzung mit nichtlinearem Kalman-Filter (EKF/UKF)
  Antworten mit Zitat      
Hallo an alle,

war nicht sicher, ob das Problem eher zu Regelungstechnik oder Signalverarbeitung gepackt werden soll. Ich versuchs mal hier, ansonsten bitte verschieben Smile

Also, ich wollte mich mal an Parameterschätzung mittels Kalman-Filterung wagen. Zu Beginn auf noch eine relative simple weise. Als Testsystem habe ich dabei das "Random (Wally) Walk"-Beispiel aus folgender Quelle genommen: http://www.personal.reading.ac.uk/~.....otes_on_kalman_filter.pdf

In Zustandsraumdarstellung sieht dieses wie folgt aus:

\frac{d}{dt} \vec{x} = \begin{vmatrix} 0 & 1 & 0 & 0 \\  0 & - \frac{1}{T}  & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 &  - \frac{1}{T} \end{vmatrix} \vec{x} + \begin{pmatrix} 0 & 0 \\ 0 & 1 \\ 0 & 0 \\ 1 & 0  \end{pmatrix}  \vec{u}

Der Parameter T ist dabei einfach (irgend-)eine Zeitkonstante. Diese will ich später durch einen Kalman-Filter schätzen. Um die Parameterschätzung zu einer Zustandsschätzung zu reduzieren, kann man den Parameter ja einfach als Zustand einführen (wie z.B. in Parameter Estimation for Mechanical.....an Extended Kalman Filter beschrieben). Einziger Nachtel ist dabei, dass man dadurch i.d.R. ein nichtlineares System erhält und dadurch nur noch nicht-lineare Kalman-Filter verwenden kann.
In diskreter Form ergibt sich dann nachfolgende nichtlineare Zustandsgleichung:

\vec{x}(k+1) = \begin{pmatrix} x_{1}({k+1}) \\ x_{2}({k+1}) \\ x_{3}({k+1}) \\ x_{4}({k+1}) \\ x_{5}({k+1})  \end{pmatrix} = f(\vec{x}(k),\vec{u}(k)) = \begin{pmatrix} x_{2}({k}) \\ -\frac{1}{x_{5}(k)} x_{2}(k) + u_{2}(k) \\ x_{4}(k) \\  -\frac{1}{x_{5}(k)} x_{4}(k) + u_{1}(k) \\ x_{5}(k) \end{pmatrix}

Hoffe bisher stimmt mein Gedankengang? Die Zahl im Index soll die Nummer des Zustandes sein, durchnummeriert von x_1 bis x_5. Der Zustand x_5 entspricht dabei der Zeitkonstante T.
Da es sich um die diskrete Form handelt, wird ja nicht die Ableitung angegeben (also keine ODEs) sondern eine Gleichung für den Zustand zum nächsten Zeitpunkt. Deshalb steht in der letzten Zeile auch, dass der Zustand x_5 konstant bleibt, also zum Zeitpunkt k und k+1 gleich ist. Das passt doch so, oder?

Nun verwende ich die beiden nichtlinearen Kalman-Filter nach Yi Cao von MatLab-Central, in etwas abgeänderter Form. Die Originale gibts hier:
Extended Kalman Filter (EKF): http://www.mathworks.com/matlabcent.....lman-filter/content/ekf.m
Unscented Kalman Filter (UKF): http://www.mathworks.com/matlabcent.....lman-filter/content/ukf.m

Meine abgeäderten Varianten sind angehängt!

Sooo, nun langer Rede kurzer Sinn: ich versuche das ganze nun in einem Skript in einzelnen Schritten zu simulieren und danach mittels Kalman-Filter die Zustände zu schätzen. Das funktioniert für x_1 bis x_4 auch super. Aber ausgerechnet der Parameter (= Zustand) x_5 wird falsch geschätzt. Kann mir einer einen Tipp geben wieso und wie ich das behebe?

Mein gesamter Code sieht dann so aus (hänge es auch als MatLab-Skript an!):

Code:
%   Test EKF and UKF for 'Random Wally Walk'
clear;
clc;

%%  Configuration
n = 5;          % number of states
T = 3;          % time constant for Wally

N = 200;        % total dynamic steps

qt = 0.6;       % true std of process
rt = 0.3;       % true std of measurement

q = 0.6;        % std of process (assumed value)
r = 0.3;        % std of measurement (assumed value)
Q = q^2*eye(n); % covariance of process
R = r^2;        % covariance of measurement  

%%  Preparation

% allocate memory
sR = zeros(n,N);    % reference
sV = zeros(n,N);    % reference + process noise
zV = zeros(2,N);    % measurement
xV = zeros(n,N);    % estimate for EKF
xVu = zeros(n,N);    % estimate for UKF


%%  1. Step - Create a (ideal) Reference Simulation of our Dynamic System
%
%   This will represent our System as it would behave in an ideal world.
%   The System-Behaviour is described by the non-linear function f(x,u).
%   The System-Input is u, The System-States x.

f = @(x,u)[x(2); -1/x(5) * x(2) + u(2); x(4); -1/x(5) * x(4) + u(1); x(5)];
              % non-linear state equation for 'random wally walk'

% Simulate a walk of N steps where the input is a [0,1] random variable
u=randn(N,2);


s_init = [0;0;1;0;T];
              % initial state

s = s_init;

% Now let's calculate all N simulation steps for our discrete System
for k = 1:N
  sR(:,k)= s;                      % save actual state
  s = f(s,u(k,:));                 % update process
end


%%  2. Step - Add Process Noise to the Dynamic System
%
%   We simulated the ideal behaviour of our System in Step 1.
%   Because of NOT ideal conditions, we have deviations between
%   ideal and non-ideal behaviour. This is to be represented here
%   with the true random process noise qt.

for k = 1:N
  sV(:,k)= sR(:,k) + [qt * randn(n-1,1); 0];    % save state of reference + process noise
end


%%  3. Step - Make Measurement's (with Measurement Noise)
%
%   Our measurement's are defined in our measurement equation h(x,u).
%  

h=@(x,u)[x(1); x(3)];
              % measurement equation
              % we will be able to make a direct measure of
              % only the x/y positions


for k = 1:N
  z = h(sV(:,k),u(k,:)) + rt * randn(2,1);   % make measurement from the reference + process noise by passing it to h(x,u)
                                             % additionally we have to add the true measurement noise rt

  zV(:,k)  = z;                              % save measurments
end


%%  4. Step - Do the Filtering with the EKF (Extended Kalman Filter)
%
%

x = s_init + [qt * randn(n-1,1); 0];
              % initial state with process noise

P = eye(n);
              % initial state covariance
             
for k = 1:N
  [x, P] = ekf(f,x,u(k,:),P,h,zV(:,k),Q,R);     % ekf
  xV(:,k) = x;                                  % save estimate
end


%%  5. Step - Do the Filtering with the UKF (Unscented Kalman Filter)
%
%

x = s_init + [qt * randn(n-1,1); 0];
              % initial state with process noise

P = eye(n);
              % initial state covariance
             
for k = 1:N
  [x, P] = ukf(f,x,u(k,:),P,h,zV(:,k),Q,R);     % ukf
  xVu(:,k) = x;                                  % save estimate
end


%%  6. Step - Plot
%
%   Make Plots of everything done above.
%  

subplot(n,1,1)
plot(1:N, sR(1,:), 'r-', 1:N, sV(1,:), 'b-.', 1:N, zV(1,:), 'g-', 1:N, xV(1,:), 'k:', 1:N, xVu(1,:), 'm--')
legend('Reference', 'Reference + Process Noise', 'Measurement', 'EKF', 'UKF');
subplot(n,1,2)
plot(1:N, sR(2,:), 'r-', 1:N, sV(2,:), 'b-.', 1:N, xV(2,:), 'k:', 1:N, xVu(2,:), 'm--')
subplot(n,1,3)
plot(1:N, sR(3,:), 'r-', 1:N, sV(3,:), 'b-.', 1:N, zV(2,:), 'g-', 1:N, xV(3,:), 'k:', 1:N, xVu(3,:), 'm--')
subplot(n,1,4)
plot(1:N, sR(4,:), 'r-', 1:N, sV(4,:), 'b-.', 1:N, xV(4,:), 'k:', 1:N, xVu(4,:), 'm--')
subplot(n,1,5)
plot(1:N, sR(5,:), 'r-', 1:N, sV(5,:), 'b-.', 1:N, xV(5,:), 'k:', 1:N, xVu(5,:), 'm--')





Bin für jede Hilfe dankbar!

Viele Grüße!

plots.jpg
 Beschreibung:
Plots für die 5 Zustände

Download
 Dateiname:  plots.jpg
 Dateigröße:  107.46 KB
 Heruntergeladen:  2036 mal
exampleParamEst_EKF_UKF.m
 Beschreibung:
Beispiel für Parameterschätzung mittels EKF und UKF

Download
 Dateiname:  exampleParamEst_EKF_UKF.m
 Dateigröße:  3.93 KB
 Heruntergeladen:  478 mal
ukf.m
 Beschreibung:
abgeänderte Version des UKF, welcher auch die Übergabe des Inputs u erlaubt.

Download
 Dateiname:  ukf.m
 Dateigröße:  4.3 KB
 Heruntergeladen:  513 mal
ekf.m
 Beschreibung:
abgeänderte Version des EKF, welcher auch die Übergabe des Inputs u erlaubt.

Download
 Dateiname:  ekf.m
 Dateigröße:  3.11 KB
 Heruntergeladen:  484 mal

_________________

- EliteTUM
_____________________________________
Private Nachricht senden Benutzer-Profile anzeigen


Thomas84
Forum-Meister

Forum-Meister


Beiträge: 546
Anmeldedatum: 10.02.10
Wohnort: ---
Version: ---
     Beitrag Verfasst am: 06.01.2012, 08:56     Titel:
  Antworten mit Zitat      
Hallo,

das ist mal ein Paradebeispiel für eine ordentliche Fragestellung (Quellcode gepostet, Quellen zitiert, etc.).

Allerdings ist der Übergang von der DGL zur Differenzengleichung nicht richtig. Du musst die Zeitableitung richtig diskretisieren. z.B. für gilt:

x_1(k+1) = x_1(k) + \Delta t x_2(k)

Warum der Filter nicht funktioniert kann ich allerdings auf die Schnelle nicht erkennen.

viele Grüße
Thomas
Private Nachricht senden Benutzer-Profile anzeigen
 
EliteTUM
Themenstarter

Forum-Fortgeschrittener

Forum-Fortgeschrittener


Beiträge: 70
Anmeldedatum: 21.04.11
Wohnort: München
Version: ---
     Beitrag Verfasst am: 06.01.2012, 12:46     Titel:
  Antworten mit Zitat      
Hallo Thomas84,

Danke für den hinweis! Puh, daran habe ich natürlich nicht gedacht. In der Quelle des "Random Wally Walk"-Beispiels wird tatsächlich kontinuierlich gerechnet.

Da es sich leider um 2 nicht-lineare und 2 lineare DGLs handelt kann ich leider nicht einfach den c2d-Befehl von MatLab verwenden, denke ich mal...
Interessant wäre, ob ich das ganze Beispiel auf zeitkontinuierliche Rechnung ummünzen kann. Ich hoffe mal das ist aufwandsärmer.

Zum Problem der schlechten/falschen Zustands-/Parameterschätzung ist mir ein einfaches aber leider grundlegendes Stichwort eingefallen: Beobachtbarkeit! Ich habe noch nicht untersucht, ob das System in der Form beobachtbar ist. Leider ist das auch nicht so trivial wie bei linearen Systemen. Kein Wunder, dass in der Wirtschaft immer nur mit "langweiligen" linearen Systemen gerechnet wird, es ist soviel schöner/einfacher Very Happy

Habe auf die schnelle Änderungen am Code durchgeführt und die Zustände 2 und 4 ebenfalls als messbar angenommen. Die Ergebnisse sind dann immer noch nicht zufriedenstellend, aber wenigstens steigen sie nicht einfach nur an sondern der Filter scheint wirklich zu versuchen den Parameter zu schätzen.

Andere Eingabesignale (anstatt eines Random-Signals) haben bisher keine Änderungen ergeben.

Sollte ich noch Zeit haben die ich hierin investiere, werde ich weitere (hoffentlich positive) Ergebnisse posten.
_________________

- EliteTUM
_____________________________________
Private Nachricht senden Benutzer-Profile anzeigen
 
EliteTUM
Themenstarter

Forum-Fortgeschrittener

Forum-Fortgeschrittener


Beiträge: 70
Anmeldedatum: 21.04.11
Wohnort: München
Version: ---
     Beitrag Verfasst am: 06.01.2012, 12:50     Titel:
  Antworten mit Zitat      
Noch vergessen: Kann mir jemand sagen, wie ich für den fünften Zustand x_5 die DGL zeitkontinuierlich darstellen kann? Die Ableitung ist ja null, weil der Wert im idealen Fall konstant ist. zeitdiskret lässt sich das ja schön darstellen wie ich es gemacht habe. Aber zeitkontinuierlich?
_________________

- EliteTUM
_____________________________________
Private Nachricht senden Benutzer-Profile anzeigen
 
EliteTUM
Themenstarter

Forum-Fortgeschrittener

Forum-Fortgeschrittener


Beiträge: 70
Anmeldedatum: 21.04.11
Wohnort: München
Version: ---
     Beitrag Verfasst am: 10.01.2012, 21:54     Titel:
  Antworten mit Zitat      
so, Update von meiner Seite:

Habe es geschafft das EKF umzuschreiben, so dass es im zeitkontinuierlichen Bereich funktioniert - also zu einem Extended Kalman-Bucy-Filter (EKBF). Ich gehe davon aus, dass obiges System in der Form nicht beobachtbar ist. Habs aber nicht weiter untersucht.

Jedoch haben meine Untersuchungen mit dem System aus folgender Quelle sehr gut funktioniert:

[1] http://academic.csuohio.edu/simond/courses/kalman/

Man siehe sich dazu das Beispiel "Parameter.m" an.

Um Vergleiche anstellen zu können würde ich nun gerne das Unscented Kalman Filter (UKF) zu einem Unscented Kalman-Bucy-Filter (UKBF) umschreiben. Also das eigtl. diskrete Filter zu einem kontinuierlich-diskreten Filter erweitern. Wollte fragen, ob das schon mal wer gemacht hat?
Als Quelle dazu habe ich folgende Links gefunden:

[2] http://www.lce.hut.fi/~ssarkka/pub/ukf-preprint.pdf
[3] http://lib.tkk.fi/Diss/2006/isbn9512281279/

Jedoch scheine ich irgendetwas falsch zu machen.

Mein Vorgehen ist bis zum Update-Schritt exakt gleich wie beim diskreten UKF in Matrixform. Jedoch berechne ich dann das Filter-Gain K wie in Gleichung (29) gezeigt. Dann berechne ich die Ableitung von m m_dot. Dann nehme ich das m aus dem letzten Zeitschritt und addiere dazu m_dot*dt (wobei dt die Zeitabstände zwischen den Filterdurchläufen ist).
Dann berechne ich ebenfalls wie in (29) P_dot und integriere es dann auf.

Irgendwie erhalte ich aber total andere Ergebnisse für EKBF und UKBF. Der UKBF gibt mir nicht mal eine Schätzung für den Parameter.

Was mich besonders verwirrt ist, dass die Messung z(t) eine "differential measurement" sein sollen. Heißt das, ich muss die zeitbezogene Änderung der Messung nehmen (sprich: diskrete Zeitableitung)?
_________________

- EliteTUM
_____________________________________
Private Nachricht senden Benutzer-Profile anzeigen
 
michi_87
Forum-Newbie

Forum-Newbie


Beiträge: 4
Anmeldedatum: 15.09.12
Wohnort: ---
Version: ---
     Beitrag Verfasst am: 28.01.2013, 14:09     Titel: Parameterschätzung mittels EKF
  Antworten mit Zitat      
Hallo!

Ich versuche grade mal das Beispiel von Yi Cao nachzuvollziehen. Ich würde später gerne eine Zustands- und Parameterschätzung für eine permanenterregte Synchronmaschine machen, und zwar soll das ganze online während einer Simulink Simulation gemacht werden. Da ich aber klein anfangen will schau ich mir erstmal den Algorithmus an. Allerdings scheitert es schon dabei.
Habe mir den Code von Yi Cao kopiert und das Beispiel was angegeben war genommen. Angezeigt wird mir allerdings immer


??? Input argument "fstate" is undefined.
Error in ==> ekf at 20
[x1,A]=jaccsd(fun,x); %nonlinear update and linearization at current state

Weiß jdm. was ich im code ändern muss, damit es funktioniert? Wäre euch sehr dankbar.

Gruß!

Michael

Code:

function [x,P]=ekf(fstate,x,P,hmeas,z,Q,R)
% EKF   Extended Kalman Filter for nonlinear dynamic systems
% [x, P] = ekf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P
% for nonlinear dynamic system:
%           x_k+1 = f(x_k) + w_k
%           z_k   = h(x_k) + v_k
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
%       v ~ N(0,R) meaning v is gaussian noise with covariance R
% Inputs:   f: function handle for f(x)
%           x: "a priori" state estimate
%           P: "a priori" estimated state covariance
%           h: fanction handle for h(x)
%           z: current measurement
%           Q: process noise covariance
%           R: measurement noise covariance
% Output:   x: "a posteriori" state estimate
%           P: "a posteriori" state covariance
%

[x1,A]=jaccsd(fstate,x);    %nonlinear update and linearization at current state
P=A*P*A'+Q;                 %partial update
[z1,H]=jaccsd(hmeas,x1);    %nonlinear measurement and linearization
P12=P*H';                   %cross covariance
% K=P12*inv(H*P12+R);       %Kalman filter gain
% x=x1+K*(z-z1);            %state estimate
% P=P-K*P12';               %state covariance matrix
R=chol(H*P12+R);            %Cholesky factorization
U=P12/R;                    %K=U/R'; Faster because of back substitution
x=x1+U*(R'\(z-z1));         %Back substitution to get state update
P=P-U*U';                   %Covariance update, U*U'=P12/R/R'*P12'=K*P12.

function [z,A]=jaccsd(fun,x)
% JACCSD Jacobian through complex step differentiation
% [z J] = jaccsd(f,x)
% z = f(x)
% J = f'(x)
%
z=fun(x);
n=numel(x);
m=numel(z);
A=zeros(m,n);
h=n*eps;
for k=1:n
    x1=x;
    x1(k)=x1(k)+h*i;a
    A(:,k)=imag(fun(x1))/h;
end




n=3;      %number of state
q=0.1;    %std of process
r=0.1;    %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2;        % covariance of measurement  
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  % nonlinear state equations
h=@(x)x(1);                               % measurement equation
s=[0;0;1];                                % initial state
x=s+q*randn(3,1); %initial state          % initial state with noise
P = eye(n);                               % initial state covraiance
N=20;                                     % total dynamic steps
xV = zeros(n,N);          %estmate        % allocate memory
sV = zeros(n,N);          %actual
zV = zeros(1,N);
for k=1:N
  z = h(s) + r*randn;                     % measurments
  sV(:,k)= s;                             % save actual state
  zV(k)  = z;                             % save measurment
  [x, P] = ekf(f,x,P,h,z,Q,R);            % ekf
  xV(:,k) = x;                            % save estimate
  s = f(s) + q*randn(3,1);                % update process
end
for k=1:3                                 % plot results
  subplot(3,1,k)
  plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')
end


 
Private Nachricht senden Benutzer-Profile anzeigen
 
EliteTUM
Themenstarter

Forum-Fortgeschrittener

Forum-Fortgeschrittener


Beiträge: 70
Anmeldedatum: 21.04.11
Wohnort: München
Version: ---
     Beitrag Verfasst am: 29.01.2013, 11:19     Titel:
  Antworten mit Zitat      
Hallo,

wie rufst du die Funktion auf? Es handelt sich um eine allgemeine Funktion, kein Skript. D.h. du musst zuerst die Übergabeargumente an die Funktion festlegen (wie in meinem Beispiel) und DANN damit die Funktion aufrufen

In meinem geposteten Beispiel wird der EKF im 4. Step in der Zeile

Code:
[x, P] = ekf(f,x,u(k,:),P,h,zV(:,k),Q,R);     % ekf


aufgerufen.
_________________

- EliteTUM
_____________________________________
Private Nachricht senden Benutzer-Profile anzeigen
 
michi_87
Forum-Newbie

Forum-Newbie


Beiträge: 4
Anmeldedatum: 15.09.12
Wohnort: ---
Version: ---
     Beitrag Verfasst am: 30.01.2013, 11:02     Titel:
  Antworten mit Zitat      
Habs herausgefunden. Musste es einfach nur als Funktion aufrufen.

Jetzt hab ich aber ein ganz anderes Problem. Wie schon erwähnt versuche ich eine Parameterschätzung für eine Synchronmaschine zu implementieren.Dazu habe ich den oberen Code als Vorlage genommen, da dieser eig schon sehr gut ist.
Zunächst einmal will ich die Zustände der Maschine schätzen, bzw. die beiden Ströme werden gemessen, die verbleibenden Zustände also Drehzahl bzw. Winkelgeschwindigkeit und den Winkel will ich schätzen. Habe also ins. 4 Zustände. Gemessen wird natürlich mit einem gewissen Messrauschen, das Prozessrauschen hab ich mal sehr klein gewählt. Die Simulation von dem Modell selber funktioniert ganz gut. Kommt die EKF Schätzung hinzu funktioniert das für kleine Werte für die Anzahl der Simulationsschritte auch ganz ok, obwohl ich glaube, dass der EKf da schon nicht richtig schätzt. ollte eigentlich genauer sein. Für größere zeigt Matlab an, dass eine Matrix im ekf Algorithmus singulär wird. Hab mir mal mal die Rückgabewerte von dem Algorithmus angeschaut und die Kovarianzmatrix P wird ab einem gewissen Punkt sehr groß. Hat jmd. vllt nen Tipp voran das liegen könnte? Hab den Code mal unten angehängt. Hab leider keinen Anhaltspunkt wie ich da weiterkomme...Wäre super wenn jmd. vllt ne Idee hätte. Dafür wäre ich sehr dankbar!

Code:



clc;
%% Parameter

Rs = 0.01;     %Statorwiderstand
Ld = 4.8e-5;       %Induktivität Ld
Lq = 5.8e-5;       %Induktivität Lq
zp =  5;       %Polpaarzahl
Psipm = 0.006;   % Permanentfluss
J     = 1.4e-4;      % Trägheitsmoment;

T = 0.00001;

a1 = -Rs/Ld;
a2 = (Lq/Ld)*zp;
a3 = (-Ld/Lq)*zp;
a4 = -Rs/Lq;
a5 = -zp*Psipm/Lq;
a6 = 3/(2*J)*zp*Psipm;
a7 = 3/(2*J)*(Ld-Lq);
b1 = 1/Ld;
b2 = 1/Lq;
%%  Configuration
n = 4;          % number of states

N = 500;        % total dynamic steps

qt = 1;       % true std of process
rt = 5;       % true std of measurement

q = 0.5;        % std of process (assumed value)
r = 2;        % std of measurement (assumed value)
Q = q^2*eye(n); % covariance of process
R = r^2;        % covariance of measurement  

%%  Preparation

% allocate memory
sR = zeros(n,N);    % reference
sV = zeros(n,N);    % reference + process noise
zV = zeros(2,N);    % measurement
xV = zeros(n,N);    % estimate for EKF



%%  1. Step - Create a (ideal) Reference Simulation of our Dynamic System
%
%   This will represent our System as it would behave in an ideal world.
%   The System-Behaviour is described by the non-linear function f(x,u).
%   The System-Input is u, The System-States x.

f = @(x,u)[(1+T*a1)*x(1)+T*a2*x(2)*x(3)+T*b1*u(1);
    T*a3*x(1)*x(3)+(1+T*a4)*x(2)+T*a5*x(3)+b2*T*u(2);
    (T*a6+T*a7*x(1))*x(2)+x(3);
    T*x(3)+x(4)];
              % non-linear state equation for 'random wally walk'

% Simulate a walk of N steps where the input is a [0,1] random variable
u=[1;1];


s_init = [0;0;0;0];
              % initial state

s = s_init;

% Now let's calculate all N simulation steps for our discrete System
for k = 1:N
  sR(:,k)= s;                      % save actual state
  s = f(s,u);              % update process
end


%%  2. Step - Add Process Noise to the Dynamic System
%
%   We simulated the ideal behaviour of our System in Step 1.
%   Because of NOT ideal conditions, we have deviations between
%   ideal and non-ideal behaviour. This is to be represented here
%   with the true random process noise qt.

for k = 1:N
  sV(:,k)= sR(:,k) + [qt * randn(n-1,1); 0];    % save state of reference + process noise
end

%
%%  3. Step - Make Measurement's (with Measurement Noise)
%
%   Our measurement's are defined in our measurement equation h(x,u).
%  

h=@(x,u)[x(1); x(2)];
              % measurement equation
              % we will be able to make a direct measure of
              % only the x/y positions


for k = 1:N
  z = h(sV(:,k),u) + rt * randn(2,1);   % make measurement from the reference + process noise by passing it to h(x,u)
                                             % additionally we have to add the true measurement noise rt

  zV(:,k)  = z;                              % save measurments
end


%%  4. Step - Do the Filtering with the EKF (Extended Kalman Filter)
%
%

x = s_init + [qt * randn(n-1,1); 0];
              % initial state with process noise

P =eye(4,4);
              % initial state covariance
             
for k = 1:N
  [x, P] = ekf2(f,x,u,P,h,zV(:,k),Q,R)   % ekf
  xV(:,k) = x;                                  % save estimate
end




%%  6. Step - Plot
%
%   Make Plots of everything done above.
%  

subplot(n,1,1)
plot(1:N, sR(1,:), 'r-', 1:N, sV(1,:), 'c-.', 1:N, zV(1,:), 'b-',1:N,xV(1,:), 'm')
grid on;
subplot(n,1,2)
plot(1:N, sR(2,:), 'r-', 1:N, sV(2,:), 'c-.', 1:N,zV(2,:), 'b-',1:N,xV(2,:), 'm')
grid on;
subplot(n,1,3)
plot(1:N, sR(3,:), 'r-', 1:N, sV(3,:), 'c-.',1:N,xV(3,:), 'm')
grid on;
subplot(n,1,4)
plot(1:N, sR(4,:), 'r-', 1:N, sV(4,:), 'c-.',1:N,xV(4,:), 'm')
grid on;
 
Private Nachricht senden Benutzer-Profile anzeigen
 
Runkelhuhn
Forum-Newbie

Forum-Newbie


Beiträge: 2
Anmeldedatum: 06.03.13
Wohnort: ---
Version: ---
     Beitrag Verfasst am: 06.03.2013, 18:36     Titel:
  Antworten mit Zitat      
Hallo,

gut das es das Thema schon gibt, leider habe ich ein anderes Problem.

Ich möchte mit einem erweiterten Kalman Filter die Masse eines Pendels schätzen, wobei es sich um einen Trägheitsbehafteten Stab handelt, der an einem Gelenk befestigt ist. Das Gelenk wird mit einem Moment angeregt (Sinus-Schwingung). Das ganze soll später eine Art Roborarm geben, ich habe nur vereinfacht ersteinmal mit einem Gelenk und einem Stab angefangen.

Mein Problem ist, das die geschätzte Winkelgeschwindigkeit des Filters eine andere Amplitude aufzeigt als sie in Wirklichkeit hat. Dadurch Schätzt der Filter eine falsche Masse, da, obwohl die richtige Masse als Anfangswert vorliegt, der Amplitudenabweichungen zu einer Anpassung der Masse seitens des Filters führt.
Mir ist aufgefallen, das wenn ich die beiden Therme der Erdbeschl. und des Anregermomentes mit 0,573 multipliziere die Amplitude passt und die Masse richtig geschätzt wird. Ändere ich allerdings die Anregerfrequenz oder die Amplitude, so schätzt der Filter die Masse wie folgt Falsch:

10000*sin(2*pi*1) in [Nm] ergibt geschätzte Masse ~ 600kg (richtiger Wert)
10000*sin(2*pi*0,8 ) ergibt geschätzte Masse ~ 590 kg
15000*sin(2*pi*0,8 ) ergibt ~580 kg

Ich habe nun also das Gefühl, das ich vll. etwas bei der Diskretisierung falsch gemacht habe bzw. der Filter aufgrund der Linearisierung vll. Fehler macht. es scheint als wenn die Dynamik das Systems nicht richtig abgebildet wird und der Filter deshalb bei unterschiedlicher Anregung verschiedene Ergebnisse liefert.

Ich habe eine Datei angehängt, dort ist mein System mit der DGL, und die Berechnung der Jacobi-Matrix sowie die Diskretisierung für den EKF zu sehen. (Habs per Hand aufgeschrieben, das geht schneller Smile )

Das Modell habe ich in Simmechanics aufgebaut, der Filter über eine embedded Function siehe:

Code:
function [phi_out, phi_dot_out, m_out, P_out] = EKF_persistent_ein_gelenk_mod(z_k,u_k)
% Erweiterter Kalman-Filter
% Zustand wird druch nichtlineare Gleichung beschrieben:
% x_k = f(x_k-1,u_k,w_k-1)
% z_k = h(x_k,v_k)
%
% Eingänge: f: f(x_k-1,u_k,0)
%           h: h(x_k,0)
%           z: Aktuelle Messung
%           x: Anfangswerte der Zustände
%           P: Anfangswerte der Kovarianzmatrix
%           Q: Kovarianz des Prozessrauschens
%           R: Kovarianz des Messrauschens
% Ausgänge: x_out: Geschätzte Zustände
%           P_out: Schätzfehler Kovarianz

persistent x_k P_k

if isempty(P_k)
    x_k = [0; 0; 650];               % Anfangswerte für x_k
    P_k = eye(3)*100;   % Anfangswerte für P_k
end

Ts = 0.001;                  % Definieren der Sampletime [s]
g = -9.81;
l = 2;
Q = eye(3)*10;     % Defninieren der Prozess-Kovarianzen
R = eye(1)*1;      % Defninieren der Mess-Kovarianzen
I = eye(3);           % Diagonalmatrix

% Prediction %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x_k = [x_k(1)+Ts*x_k(2); 0.573*(3/2)*(g/l)*cosd(x_k(1))*Ts*x_k(1)+x_k(2)+180/pi*0.573*3*Ts*u_k/(x_k(3)*l^2); x_k(3)];   % a-priori Schätzung
A_k = [1 Ts 0; 0.573*(3/2)*(g/l)*cosd(x_k(1))*Ts 1 -180/pi*0.573*3*Ts*u_k/(l^2*x_k(3)^2); 0 0 1];% Berechnung der Jacobimatrix A_k
H_k = [1 0 0];
P_k = A_k * P_k * A_k.' + Q;            % a-priori Schätzfehler-Kovarianzmatrix

% Correction %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

K_k = P_k * H_k' / (H_k * P_k * H_k.' + R);% Berechnung des Kalmangain
x_k = x_k + K_k * (z_k - H_k*x_k);% a-posteriori Schätzung lineare Messgl.
P_k = (I - K_k * H_k)*P_k;                  % a-posteriori Schätzfehler-Kovarianzmatrix

% Setzen der Ausgänge %%%%%%%%%%%%%%%%%%%%%
phi_out = x_k(1);
phi_dot_out = x_k(2);
m_out = x_k(3);
P_out = P_k;


Vielen Dank im Vorraus.

2013-03-06 18.29.50.jpg
 Beschreibung:

Download
 Dateiname:  2013-03-06 18.29.50.jpg
 Dateigröße:  1.13 MB
 Heruntergeladen:  1898 mal
2013-03-06 18.29.27.jpg
 Beschreibung:

Download
 Dateiname:  2013-03-06 18.29.27.jpg
 Dateigröße:  1.19 MB
 Heruntergeladen:  1879 mal
Private Nachricht senden Benutzer-Profile anzeigen
 
geieraffe
Forum-Fortgeschrittener

Forum-Fortgeschrittener


Beiträge: 97
Anmeldedatum: 19.04.12
Wohnort: ---
Version: ---
     Beitrag Verfasst am: 13.03.2013, 10:32     Titel:
  Antworten mit Zitat      
EliteTUM hat Folgendes geschrieben:
...
Habe es geschafft das EKF umzuschreiben, so dass es im zeitkontinuierlichen Bereich funktioniert - also zu einem Extended Kalman-Bucy-Filter (EKBF)....


Hey EliteTUM,
du hast geschrieben, dass du es geschafft hast einen EKBF zu implementieren.
Hast Du dazu eine gute Quelle, oder würdest Du nochmal einen Code posten?
Ich habe nämlich ein ähnliches Problem, bei dem ich es nicht so ganz einfach schaffe mein System zu diskretisieren.

Vielen Dank
Private Nachricht senden Benutzer-Profile anzeigen
 
Heber
Forum-Fortgeschrittener

Forum-Fortgeschrittener


Beiträge: 54
Anmeldedatum: 01.02.17
Wohnort: ---
Version: ---
     Beitrag Verfasst am: 16.02.2017, 22:26     Titel:
  Antworten mit Zitat      
Hallo, ließ sich der ursprünglich vorgestellte Ansatz auch in eine Simulink modell integrieren? Weil das macht es ja erst interessant...
Private Nachricht senden Benutzer-Profile anzeigen
 
Neues Thema eröffnen Neue Antwort erstellen



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
.


goMatlab ist ein Teil des goForen-Labels
goForen.de goMATLAB.de goLaTeX.de goPCB.de


 Impressum  | Nutzungsbedingungen  | Datenschutz  | Werbung/Mediadaten | Studentenversion | FAQ | goMatlab RSS Button RSS


Copyright © 2007 - 2017 goMatlab.de | Dies ist keine offizielle Website der Firma The Mathworks
Partner: LabVIEWforum.de

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.