some stuff

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):
-
+        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()

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,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,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}