Verfasst am: 05.01.2012, 20:56
Titel: Parameterschätzung mit nichtlinearem Kalman-Filter (EKF/UKF
Hallo an alle,
war nicht sicher, ob das Problem eher zu Regelungstechnik oder Signalverarbeitung gepackt werden soll. Ich versuchs mal hier, ansonsten bitte verschieben
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:
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:
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?
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
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
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.
_________________
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?
_________________
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:
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:
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)?
_________________
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
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
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!
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
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 )
Das Modell habe ich in Simmechanics aufgebaut, der Filter über eine embedded Function siehe:
ifisempty(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
...
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.
Hallo, ließ sich der ursprünglich vorgestellte Ansatz auch in eine Simulink modell integrieren? Weil das macht es ja erst interessant...
Einstellungen und Berechtigungen
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
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.
RSS