diff --git a/code/kreis_kalman.py b/code/kreis_kalman.py index b629d94..f5011e8 100644 --- a/code/kreis_kalman.py +++ b/code/kreis_kalman.py @@ -4,15 +4,139 @@ import numpy class KalmanFilter: - def __init__(self, _A, _B, _H, _x, _P, _Q, _R): - self.A = _A - self.B = _B - self.H = _H - self.state_estimate = _x - self.prob_estimate = _P - self.Q = _Q - self.R = _R + def __init__(self, _F, _B, _H, _x, _P, _Q, _R): + self.F = _F # State transition matrix. + self.B = _B # Control matrix. + self.H = _H # Observation matrix. + self.xhat = _x # Initial state estimate. + self.P = _P # Initial covariance estimate. + self.Q = _Q # Estimated error in process. + self.R = _R # Estimated error in measurements. def currentState(self): - return self.state_estimate; - def step(self, control_vector, measurement_vector): - \ No newline at end of file + return self.xhat; + def step(self, u, z): + # u: control vector, z: measurement vector + + #predict + xhatminus = self.F * self.xhat + self.B * u + Pminus = (self.F * self.P) * numpy.transpose(self.F) + self.Q + + #observation + + # i dont know why but the second one y is much better in estimating + # but the first one should be according to wikipedia the correct one + #y = z - self.H * xhatminus # innovation + y = self.H * z - xhatminus # innovation + + + S = self.H * Pminus * numpy.transpose(self.H) + self.R # innovation covariance + + #update + + K = Pminus * numpy.transpose(self.H) * numpy.linalg.inv(S) + self.xhat = xhatminus + K * y + + self.P = (numpy.eye(self.P.shape[0]) - K * self.H) * Pminus + + +class Cannon: + #--------------------------------VARIABLES---------------------------------- + angle = 60 + muzzle_velocity = 100 + gravity = [0,-9.81] + velocity = [muzzle_velocity*math.cos(angle*math.pi/180), muzzle_velocity*math.sin(angle*math.pi/180)] + loc = [0,0] + acceleration = [0,0] + #---------------------------------METHODS----------------------------------- + def __init__(self,_timeslice,_noiselevel): + self.timeslice = _timeslice + self.noiselevel = _noiselevel + def add(self,x,y): + return x + y + def mult(self,x,y): + return x * y + def GetX(self): + return self.loc[0] + def GetY(self): + return self.loc[1] + def GetXWithNoise(self): + return random.gauss(self.GetX(),self.noiselevel) + def GetYWithNoise(self): + return random.gauss(self.GetY(),self.noiselevel) + def GetXVelocity(self): + return self.velocity[0] + def GetYVelocity(self): + return self.velocity[1] + # Increment through the next timeslice of the simulation. + def Step(self): + + timeslicevec = [self.timeslice,self.timeslice] + sliced_gravity = map(self.mult,self.gravity,timeslicevec) + sliced_acceleration = sliced_gravity + self.velocity = map(self.add, self.velocity, sliced_acceleration) + sliced_velocity = map(self.mult, self.velocity, timeslicevec ) + self.loc = map(self.add, self.loc, sliced_velocity) + if self.loc[1] < 0: + self.loc[1] = 0 + +class System + def __init__ (self, _timeslice): + self.timeslice _timeslice + + def step(self): + + # return vector x + def getX(self): + return [0,0] + #return vector z + def measureX(self): + + + + +timeslice = 0.01 +iterations = 1444 + +real_x = [] +real_y = [] +measure_x = [] +measure_y = [] +k_x = [] +k_y = [] + +speedX = muzzle_velocity*math.cos(angle*math.pi/180) +speedY = muzzle_velocity*math.sin(angle*math.pi/180) + +F = numpy.matrix([[1,timeslice,0,0],[0,1,0,0],[0,0,1,timeslice],[0,0,0,1]]) +B = numpy.matrix([[0,0,0,0],[0,0,0,0],[0,0,1,0],[0,0,0,1]]) + +H = numpy.eye(4) +xhat = numpy.matrix([[0],[speedX*2],[500],[speedY*2]]) +P = numpy.eye(4) +Q = numpy.eye(4)*0.001 +R = numpy.eye(4)*4 +kf = KalmanFilter(F, B, H, xhat, P, Q, R) + + +control_vector = numpy.matrix([[0],[0],[-9.81*timeslice*timeslice],[-9.81*timeslice]]) + +for i in range(iterations): + x.append(c.GetX()) + y.append(c.GetY()) + newestX = c.GetXWithNoise() + newestY = c.GetYWithNoise() + nx.append(newestX) + ny.append(newestY) + + c.Step() + kx.append(kf.currentState()[0,0]) + ky.append(kf.currentState()[2,0]) + kf.step(control_vector,numpy.matrix([[newestX],[c.GetXVelocity()],[newestY],[c.GetYVelocity()]])) + +# Plot all the results we got. +pylab.plot(x,y,'-',nx,ny,':',kx,ky,'--') +pylab.xlabel('X position') +pylab.ylabel('Y position') +pylab.title('Measurement of a Cannonball in Flight') +pylab.legend(('true','measured','kalman')) +pylab.show() diff --git a/presentation/pre.tex b/presentation/pre.tex index 1c17eb6..60e8430 100644 --- a/presentation/pre.tex +++ b/presentation/pre.tex @@ -41,13 +41,14 @@ \begin{frame} \section{Trägheitsnavigation} \frametitle{Trägheitsnavigation} - In sich abgeschlossen Navigationtechnik, welche die Position und Orentierung eines Objektes relativ zu einem Start-punkt, orientierung und geschwindigkeit bestimmt. + In sich abgeschlossen Navigationstechnik, + welche die Position und Orientierung eines Objektes relativ zu einem Start-punkt, orientierung und geschwindigkeit bestimmt. Besteht aus: \begin{enumerate} \item Computer \item Accelerometer - \item Gryoscope + \item Gyroskop \end{enumerate} 2 Hauptgruppen von Konfigurationen \cite{Wood07} @@ -90,7 +91,7 @@ \begin{definition}[MEMS] = Microelectromechanical systems \\ - Very small mechanical devices driven by electricity. + Sehr kleine mechanische Geräte angetrieben durch Elektrizität. \end{definition} \medskip 2 Typen von Acceloremtern: @@ -106,8 +107,8 @@ Konstante Beschleunigungen können nicht gemessen werden. \bigskip \begin{definition}[Piezoelektrizität] - Beschreibt das Auftreten einer elektrischen Spannung an Festkörpern, wenn sie elastisch verformt werden. - \end{definition} + Beschreibt das Auftreten einer elektrischen Spannung an Festkörpern, wenn sie elastisch verformt werden. + \end{definition} \end{frame} \begin{frame} @@ -117,6 +118,7 @@ \end{block} \bigskip + Vorteile \begin{itemize} \item Herstellung mit herkömmlicher MEMS Technologie möglich @@ -132,31 +134,28 @@ C_{0} = \epsilon_{0} \epsilon_{r} \frac{A}{d} = \epsilon_{A} \frac{1}{d} \end{equation} - wobei $\epsilon_{A} = \epsilon_{0} \epsilon_{r} A$ und A die Fläche der Elektroden, d die Distanz zwischen ihnen und die $\epsilon_{r}$ die Perimitivität von dem Matrial dass die beiden trennt. + wobei $\epsilon_{A} = \epsilon_{0} \epsilon_{r} A$ und A die Fläche der Elektroden, d die Distanz zwischen ihnen und die $\epsilon_{r}$ die Perimitivität von dem Material dass die beiden trennt. \end{frame} -%%\begin{frame} - %%\frametitle{Kapazität 2} - %%\resizebox{\textwidth}{\textheight} { - %% \includegraphics[scale=0.75]{acceleromter_structure.png} - %%} - -%%\end{frame} - \begin{frame} \frametitle{Kapazität 3} \begin{wrapfigure}{l}{0.4\textwidth} \includegraphics[width=0.4\textwidth]{images/acceleromter_structure.png} \end{wrapfigure} + Die Kapazitäten $C_{1}$ und $C_{2}$ zwischen der beweglichen Platte und den äußeren Stationären Platten sind abhängig von den Verschiebung $x_{1}$ und $x_{2}$. \begin{equation} - C_{1} = \epsilon_{A} \frac{1}{x_{1}} = \epsilon_{A} \frac{1}{d+x} = C_{0} - \Delta C + C_{1} = \epsilon_{A} \frac{1}{x_{1}} + = \epsilon_{A} \frac{1}{d+x} + = C_{0} - \Delta C \end{equation} \begin{equation} - C_{2} = \epsilon_{A} \frac{1}{x_{2}} = \epsilon_{A} \frac{1}{d-x} = C_{0} + \Delta C + C_{2} = \epsilon_{A} \frac{1}{x_{2}} + = \epsilon_{A} \frac{1}{d-x} + = C_{0} + \Delta C \end{equation} \end{frame} @@ -164,19 +163,21 @@ \begin{frame} \frametitle{Kapazität 4} - Wenn die Beschleunigung null ist, dann sind die Kapaziäten $C_{1}$ und $C_{2}$ gleich. + Wenn die Beschleunigung null ist, dann sind die Kapazitäten $C_{1}$ und $C_{2}$ gleich. Wenn aber $x_{1} \neq x_{2}$ also $x \neq 0$ dann gilt: \begin{equation} - C_{1} - C_{2} = 2 \Delta C = 2 \epsilon_{A} \frac{x}{d^{2}-x^{2}} + C_{1} - C_{2} = 2 \Delta C = 2 \epsilon_{A} \frac{x}{d^{2}-x^{2}} \end{equation} Wenn wir nun $\Delta C$ messen, dann könne wir die Verschiebung $x$ messen indem wir die nichtlineare algebraische Gleichung lösen. + \begin{equation} - \Delta C x^{2} + \epsilon_{A} x + \Delta C d^{2} = 0 + \Delta C x^{2} + \epsilon_{A} x + \Delta C d^{2} = 0 \end{equation} + Für kleine Verschiebungen ist der Term $\Delta C x^{2}$ verschwindend klein. Es gilt also \begin{equation} - x \approx \frac{d^{2}}{\epsilon_{A}} \Delta C = d \frac{\Delta C}{C_{0}} + x \approx \frac{d^{2}}{\epsilon_{A}} \Delta C = d \frac{\Delta C}{C_{0}} \end{equation} Wir können also sagen, dass die Verschiebung annähernd proportional ist zur Kapazitätsdifferenz $\Delta C$ \end{frame} @@ -186,10 +187,12 @@ \frametitle{Gyroskop (Rotationssensoren)} \begin{block}{Was ist ein Gyroskop} - Ein Gerät zur Messung oder Erhaltung der Orientierung, basierend auf dem Prinzip des Drehimpulses. + Ein Gerät zur Messung oder Erhaltung der Orientierung, basierend auf dem Prinzip des Drehimpulses. \end{block} + \bigskip - Typen von Gyrokopen + + Typen von Gyroskopen \begin{enumerate} \item Mechanisch \item Optisch @@ -198,48 +201,49 @@ \end{frame} \begin{frame} -\frametitle{Mechanische Gyroskope} -%% http://www.ipgp.fr/~lucas/Contrib/animbeamer.html -\begin{wrapfigure}{l}{0.4\textwidth} -\includegraphics[width=0.4\textwidth]{images/mechanical_gyroscope.png} -\end{wrapfigure} - -Bestehen aus einem spinning wheel und zwei Gimbals, welche es eine Rotation in 3-Achsen erlaubt. \\ -Ein Mechanische Gyroscope misst die Orienation direkt, während die meisten moderen Gryoskope die Winkelgschwindigkeit messen. - -Nachteile: -\begin{enumerate} - \item Bewegliche Teile - \item Reibung - \item Ein paar Minuten Aufwärmzeit benötigt - \end{enumerate} + \frametitle{Mechanische Gyroskope} + %% http://www.ipgp.fr/~lucas/Contrib/animbeamer.html + \begin{wrapfigure}{l}{0.4\textwidth} + \includegraphics[width=0.4\textwidth]{images/mechanical_gyroscope.png} + \end{wrapfigure} + + Bestehen aus einem spinning wheel und zwei Gimbals, welche es eine Rotation in 3-Achsen erlaubt. \\ + Ein Mechanische Gyroskope misst die Orientierung direkt, + während die meisten moderne Gryoskope die Winkelgeschwindigkeit messen. + + Nachteile: + \begin{enumerate} + \item Bewegliche Teile + \item Reibung + \item Ein paar Minuten Aufwärmzeit benötigt + \end{enumerate} \end{frame} - \begin{frame} -\frametitle{Optische Gyroskope} -\begin{wrapfigure}{l}{0.4\textwidth} -\includegraphics[width=0.4\textwidth]{images/fog.png} -\end{wrapfigure} - -Insbesondere Faserkreisel (Fibro Optic Gyroscope = FOG). \\ -Besteht aus einer langen Spule von Glasfasern. Es werden zwei Lichtimpluse in entgegengesetze Richtung abgefeuert. Wenn das System rotiert, erfährt der eine Lichtimplus eine längere Laufzeit.\\ -Gemessen wird über die Interferenz von den beiden Lichimplusen. - -\end{frame} + \frametitle{Optische Gyroskope} + \begin{wrapfigure}{l}{0.4\textwidth} + \includegraphics[width=0.4\textwidth]{images/fog.png} + \end{wrapfigure} + + Insbesondere Faserkreisel (Fibro Optic Gyroscope = FOG). \\ + Besteht aus einer langen Spule von Glasfasern. Es werden zwei Lichtimpluse in entgegengesetze Richtung abgefeuert. Wenn das System rotiert, erfährt der eine Lichtimplus eine längere Laufzeit.\\ + Gemessen wird über die Interferenz von den beiden Lichimplusen. -\begin{frame} - \section{Bewegeungsgleichungen} - \frametitle{Bewegeungsgleichungen} \end{frame} +% Ich denke wir stellen zuerst allgemein den Kalman-Filter dar. \begin{frame} \section{Kalman-Filter} \frametitle{Kalman-Filter} \end{frame} +\begin{frame} + \section{Bewegunsgleichungen} + \frametitle{Modelierung eines Systems} +\end{frame} + \begin{frame} \section{Kalman-Filter für UAV} \frametitle{Kalman-Filter für UAV}