Permalink
Browse files

some stuff

  • Loading branch information...
1 parent 5ec5bb9 commit e1bad1aa98e5b9c1905b56f6a972cd54a7d08e50 @metaxy committed Mar 10, 2012
Showing with 190 additions and 62 deletions.
  1. +135 −11 code/kreis_kalman.py
  2. +55 −51 presentation/pre.tex
View
@@ -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):
-
+ 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()
View
@@ -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,51 +134,50 @@
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}
\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,49 +201,50 @@
\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}
\end{frame}

0 comments on commit e1bad1a

Please sign in to comment.