Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse code

some stuff

  • Loading branch information...
commit e1bad1aa98e5b9c1905b56f6a972cd54a7d08e50 1 parent 5ec5bb9
Paul Walger authored March 10, 2012
146  code/kreis_kalman.py
@@ -4,15 +4,139 @@
4 4
 import numpy
5 5
 
6 6
 class KalmanFilter:
7  
-    def __init__(self, _A, _B, _H, _x, _P, _Q, _R):
8  
-        self.A = _A
9  
-        self.B = _B
10  
-        self.H = _H
11  
-        self.state_estimate = _x
12  
-        self.prob_estimate = _P
13  
-        self.Q = _Q
14  
-        self.R = _R
  7
+    def __init__(self, _F, _B, _H, _x, _P, _Q, _R):
  8
+        self.F = _F             # State transition matrix.
  9
+        self.B = _B             # Control matrix.
  10
+        self.H = _H             # Observation matrix.
  11
+        self.xhat = _x          # Initial state estimate.
  12
+        self.P = _P # Initial covariance estimate.
  13
+        self.Q = _Q             # Estimated error in process.
  14
+        self.R = _R             # Estimated error in measurements.
15 15
     def currentState(self):
16  
-        return self.state_estimate;
17  
-    def step(self, control_vector, measurement_vector):
18  
-        
  16
+        return self.xhat;
  17
+    def step(self, u, z):
  18
+        # u: control vector, z: measurement vector
  19
+        
  20
+        #predict
  21
+        xhatminus = self.F * self.xhat + self.B * u
  22
+        Pminus = (self.F * self.P) * numpy.transpose(self.F) + self.Q
  23
+        
  24
+        #observation
  25
+        
  26
+        # i dont know why but the second one y is much better in estimating
  27
+        # but the first one should be according to wikipedia the correct one
  28
+        #y = z - self.H * xhatminus # innovation
  29
+        y = self.H * z - xhatminus # innovation
  30
+        
  31
+        
  32
+        S = self.H * Pminus * numpy.transpose(self.H) + self.R # innovation covariance
  33
+        
  34
+        #update
  35
+        
  36
+        K = Pminus * numpy.transpose(self.H) * numpy.linalg.inv(S)
  37
+        self.xhat = xhatminus + K * y
  38
+        
  39
+        self.P = (numpy.eye(self.P.shape[0]) - K * self.H) * Pminus
  40
+        
  41
+        
  42
+class Cannon:
  43
+    #--------------------------------VARIABLES----------------------------------
  44
+    angle = 60
  45
+    muzzle_velocity = 100
  46
+    gravity = [0,-9.81]
  47
+    velocity = [muzzle_velocity*math.cos(angle*math.pi/180), muzzle_velocity*math.sin(angle*math.pi/180)]
  48
+    loc = [0,0]
  49
+    acceleration = [0,0]
  50
+    #---------------------------------METHODS-----------------------------------
  51
+    def __init__(self,_timeslice,_noiselevel):
  52
+        self.timeslice = _timeslice
  53
+        self.noiselevel = _noiselevel
  54
+    def add(self,x,y):
  55
+        return x + y
  56
+    def mult(self,x,y):
  57
+        return x * y
  58
+    def GetX(self):
  59
+        return self.loc[0]
  60
+    def GetY(self):
  61
+        return self.loc[1]
  62
+    def GetXWithNoise(self):
  63
+        return random.gauss(self.GetX(),self.noiselevel)
  64
+    def GetYWithNoise(self):
  65
+        return random.gauss(self.GetY(),self.noiselevel)
  66
+    def GetXVelocity(self):
  67
+        return self.velocity[0]
  68
+    def GetYVelocity(self):
  69
+        return self.velocity[1]
  70
+    # Increment through the next timeslice of the simulation.
  71
+    def Step(self):
  72
+
  73
+        timeslicevec = [self.timeslice,self.timeslice]
  74
+        sliced_gravity = map(self.mult,self.gravity,timeslicevec)
  75
+        sliced_acceleration = sliced_gravity
  76
+        self.velocity = map(self.add, self.velocity, sliced_acceleration)
  77
+        sliced_velocity = map(self.mult, self.velocity, timeslicevec )
  78
+        self.loc = map(self.add, self.loc, sliced_velocity)
  79
+        if self.loc[1] < 0:
  80
+            self.loc[1] = 0
  81
+            
  82
+class System
  83
+    def __init__ (self, _timeslice):
  84
+        self.timeslice _timeslice
  85
+    
  86
+    def step(self):
  87
+    
  88
+    # return vector x
  89
+    def getX(self):
  90
+        return [0,0]
  91
+    #return vector z
  92
+    def measureX(self):
  93
+        
  94
+        
  95
+
  96
+
  97
+timeslice = 0.01
  98
+iterations = 1444 
  99
+
  100
+real_x = []
  101
+real_y = []
  102
+measure_x = []
  103
+measure_y = []
  104
+k_x = []
  105
+k_y = []
  106
+
  107
+speedX = muzzle_velocity*math.cos(angle*math.pi/180)
  108
+speedY = muzzle_velocity*math.sin(angle*math.pi/180)
  109
+
  110
+F = numpy.matrix([[1,timeslice,0,0],[0,1,0,0],[0,0,1,timeslice],[0,0,0,1]])
  111
+B = numpy.matrix([[0,0,0,0],[0,0,0,0],[0,0,1,0],[0,0,0,1]])
  112
+
  113
+H = numpy.eye(4)
  114
+xhat = numpy.matrix([[0],[speedX*2],[500],[speedY*2]])
  115
+P = numpy.eye(4)
  116
+Q = numpy.eye(4)*0.001
  117
+R = numpy.eye(4)*4
  118
+kf = KalmanFilter(F, B, H, xhat, P, Q, R)
  119
+
  120
+
  121
+control_vector = numpy.matrix([[0],[0],[-9.81*timeslice*timeslice],[-9.81*timeslice]])
  122
+
  123
+for i in range(iterations):
  124
+    x.append(c.GetX())
  125
+    y.append(c.GetY())
  126
+    newestX = c.GetXWithNoise()
  127
+    newestY = c.GetYWithNoise()
  128
+    nx.append(newestX)
  129
+    ny.append(newestY)
  130
+
  131
+    c.Step()
  132
+    kx.append(kf.currentState()[0,0])
  133
+    ky.append(kf.currentState()[2,0])
  134
+    kf.step(control_vector,numpy.matrix([[newestX],[c.GetXVelocity()],[newestY],[c.GetYVelocity()]]))
  135
+
  136
+# Plot all the results we got.
  137
+pylab.plot(x,y,'-',nx,ny,':',kx,ky,'--')
  138
+pylab.xlabel('X position')
  139
+pylab.ylabel('Y position')
  140
+pylab.title('Measurement of a Cannonball in Flight')
  141
+pylab.legend(('true','measured','kalman'))
  142
+pylab.show()
106  presentation/pre.tex
@@ -41,13 +41,14 @@
41 41
 \begin{frame}
42 42
 	\section{Trägheitsnavigation}
43 43
 	\frametitle{Trägheitsnavigation}
44  
-	In sich abgeschlossen Navigationtechnik, welche die Position und Orentierung eines Objektes relativ zu einem Start-punkt, orientierung und geschwindigkeit bestimmt.
  44
+	In sich abgeschlossen Navigationstechnik, 
  45
+	welche die Position und Orientierung eines Objektes relativ zu einem Start-punkt, orientierung und geschwindigkeit bestimmt.
45 46
 	
46 47
 	Besteht aus:
47 48
 	\begin{enumerate}
48 49
 		\item Computer
49 50
 		\item Accelerometer
50  
-		\item Gryoscope
  51
+		\item Gyroskop
51 52
 	\end{enumerate}
52 53
 	
53 54
 	2 Hauptgruppen von Konfigurationen \cite{Wood07}
@@ -90,7 +91,7 @@
90 91
   
91 92
   \begin{definition}[MEMS]
92 93
   = Microelectromechanical systems \\
93  
-  Very small mechanical devices driven by electricity.
  94
+  Sehr kleine mechanische Geräte angetrieben durch Elektrizität.
94 95
   \end{definition}
95 96
   \medskip 
96 97
   2 Typen von Acceloremtern:
@@ -106,8 +107,8 @@
106 107
   Konstante Beschleunigungen können nicht gemessen werden.
107 108
     \bigskip
108 109
     \begin{definition}[Piezoelektrizität]
109  
-  Beschreibt das Auftreten einer elektrischen Spannung an Festkörpern, wenn sie elastisch verformt werden.
110  
-  \end{definition}
  110
+		Beschreibt das Auftreten einer elektrischen Spannung an Festkörpern, wenn sie elastisch verformt werden.
  111
+	\end{definition}
111 112
 \end{frame}
112 113
 
113 114
 \begin{frame}
@@ -117,6 +118,7 @@
117 118
 	\end{block}
118 119
 
119 120
     \bigskip
  121
+    
120 122
 	Vorteile
121 123
  	\begin{itemize}
122 124
  		\item Herstellung mit herkömmlicher MEMS Technologie möglich
@@ -132,31 +134,28 @@
132 134
 		C_{0} = \epsilon_{0} \epsilon_{r} \frac{A}{d} = \epsilon_{A} \frac{1}{d}
133 135
 	\end{equation}
134 136
 	
135  
-	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.
  137
+	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.
136 138
 
137 139
 \end{frame}
138 140
 
139  
-%%\begin{frame}
140  
-	%%\frametitle{Kapazität 2}
141  
-	%%\resizebox{\textwidth}{\textheight} {
142  
-	%%	\includegraphics[scale=0.75]{acceleromter_structure.png}
143  
-	%%}
144  
-
145  
-%%\end{frame}
146  
-
147 141
 \begin{frame}
148 142
 	\frametitle{Kapazität 3}
149 143
 
150 144
 \begin{wrapfigure}{l}{0.4\textwidth}
151 145
 \includegraphics[width=0.4\textwidth]{images/acceleromter_structure.png}
152 146
 \end{wrapfigure}
  147
+
153 148
 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}$.
154 149
 	\begin{equation}
155  
-	C_{1} = \epsilon_{A} \frac{1}{x_{1}} = \epsilon_{A} \frac{1}{d+x} = C_{0} - \Delta C
  150
+		C_{1} = \epsilon_{A} \frac{1}{x_{1}} 
  151
+			  = \epsilon_{A} \frac{1}{d+x} 
  152
+			  = C_{0} - \Delta C
156 153
 	\end{equation}
157 154
 	
158 155
 	\begin{equation}
159  
-	C_{2} = \epsilon_{A} \frac{1}{x_{2}} = \epsilon_{A} \frac{1}{d-x} = C_{0} + \Delta C
  156
+		C_{2} = \epsilon_{A} \frac{1}{x_{2}} 
  157
+		      = \epsilon_{A} \frac{1}{d-x} 
  158
+		      = C_{0} + \Delta C
160 159
 	\end{equation}
161 160
 	
162 161
 \end{frame}
@@ -164,19 +163,21 @@
164 163
 \begin{frame}
165 164
 	\frametitle{Kapazität 4}
166 165
 	
167  
-	Wenn die Beschleunigung null ist, dann sind die Kapaziäten $C_{1}$ und $C_{2}$ gleich.
  166
+	Wenn die Beschleunigung null ist, dann sind die Kapazitäten $C_{1}$ und $C_{2}$ gleich.
168 167
 	Wenn aber $x_{1} \neq x_{2}$ also $x \neq 0$ dann gilt:
169 168
 	\begin{equation}
170  
-	C_{1} - C_{2} = 2 \Delta C = 2 \epsilon_{A} \frac{x}{d^{2}-x^{2}}
  169
+		C_{1} - C_{2} = 2 \Delta C = 2 \epsilon_{A} \frac{x}{d^{2}-x^{2}}
171 170
 	\end{equation}
172 171
 	
173 172
 	Wenn wir nun $\Delta C$ messen, dann könne wir die Verschiebung $x$ messen indem wir die nichtlineare algebraische Gleichung lösen.
  173
+	
174 174
 	\begin{equation}
175  
-	\Delta C x^{2} + \epsilon_{A} x + \Delta C d^{2} = 0
  175
+		\Delta C x^{2} + \epsilon_{A} x + \Delta C d^{2} = 0
176 176
 	\end{equation}
  177
+	
177 178
 	Für kleine Verschiebungen ist der Term $\Delta C x^{2}$ verschwindend klein. Es gilt also
178 179
 	\begin{equation}
179  
-	x \approx \frac{d^{2}}{\epsilon_{A}} \Delta C = d \frac{\Delta C}{C_{0}}
  180
+		x \approx \frac{d^{2}}{\epsilon_{A}} \Delta C = d \frac{\Delta C}{C_{0}}
180 181
 	\end{equation}
181 182
 	Wir können also sagen, dass die Verschiebung annähernd proportional ist zur Kapazitätsdifferenz $\Delta C$
182 183
 \end{frame}
@@ -186,10 +187,12 @@
186 187
   \frametitle{Gyroskop (Rotationssensoren)}
187 188
   
188 189
   \begin{block}{Was ist ein Gyroskop}
189  
-  Ein Gerät zur Messung oder Erhaltung der Orientierung, basierend auf dem Prinzip des Drehimpulses.
  190
+  	Ein Gerät zur Messung oder Erhaltung der Orientierung, basierend auf dem Prinzip des Drehimpulses.
190 191
   \end{block}
  192
+  
191 193
   \bigskip
192  
-  Typen von Gyrokopen
  194
+  
  195
+  Typen von Gyroskopen
193 196
   \begin{enumerate}
194 197
   	\item Mechanisch
195 198
   	\item Optisch 
@@ -198,42 +201,38 @@
198 201
 \end{frame}
199 202
 
200 203
 \begin{frame}
201  
-\frametitle{Mechanische Gyroskope}
202  
-%% http://www.ipgp.fr/~lucas/Contrib/animbeamer.html
203  
-\begin{wrapfigure}{l}{0.4\textwidth}
204  
-\includegraphics[width=0.4\textwidth]{images/mechanical_gyroscope.png} 
205  
-\end{wrapfigure}
206  
-
207  
-Bestehen aus einem spinning wheel und zwei Gimbals, welche es eine Rotation in 3-Achsen erlaubt. \\
208  
-Ein Mechanische Gyroscope misst die Orienation direkt, während die meisten moderen Gryoskope die Winkelgschwindigkeit messen.
209  
-
210  
-Nachteile:
211  
-\begin{enumerate}
212  
-  	\item Bewegliche Teile
213  
-  	\item Reibung
214  
-  	\item Ein paar Minuten Aufwärmzeit benötigt
215  
-  \end{enumerate}
  204
+	\frametitle{Mechanische Gyroskope}
  205
+	%% http://www.ipgp.fr/~lucas/Contrib/animbeamer.html
  206
+	\begin{wrapfigure}{l}{0.4\textwidth}
  207
+	\includegraphics[width=0.4\textwidth]{images/mechanical_gyroscope.png} 
  208
+	\end{wrapfigure}
  209
+	
  210
+	Bestehen aus einem spinning wheel und zwei Gimbals, welche es eine Rotation in 3-Achsen erlaubt. \\
  211
+	Ein Mechanische Gyroskope misst die Orientierung direkt, 
  212
+	während die meisten moderne Gryoskope die Winkelgeschwindigkeit messen.
  213
+	
  214
+	Nachteile:
  215
+	\begin{enumerate}
  216
+	  	\item Bewegliche Teile
  217
+	  	\item Reibung
  218
+	  	\item Ein paar Minuten Aufwärmzeit benötigt
  219
+	  \end{enumerate}
216 220
 
217 221
 \end{frame}
218 222
 
219  
-
220 223
 \begin{frame}
221  
-\frametitle{Optische Gyroskope}
222  
-\begin{wrapfigure}{l}{0.4\textwidth}
223  
-\includegraphics[width=0.4\textwidth]{images/fog.png} 
224  
-\end{wrapfigure}
225  
-
226  
-Insbesondere Faserkreisel (Fibro Optic Gyroscope = FOG). \\
227  
-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.\\
228  
-Gemessen wird über die Interferenz von den beiden Lichimplusen.
229  
-
230  
-\end{frame}
  224
+	\frametitle{Optische Gyroskope}
  225
+	\begin{wrapfigure}{l}{0.4\textwidth}
  226
+	\includegraphics[width=0.4\textwidth]{images/fog.png} 
  227
+	\end{wrapfigure}
  228
+	
  229
+	Insbesondere Faserkreisel (Fibro Optic Gyroscope = FOG). \\
  230
+	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.\\
  231
+	Gemessen wird über die Interferenz von den beiden Lichimplusen.
231 232
 
232  
-\begin{frame}
233  
-  \section{Bewegeungsgleichungen}
234  
-  \frametitle{Bewegeungsgleichungen}
235 233
 \end{frame}
236 234
 
  235
+% Ich denke wir stellen zuerst allgemein den Kalman-Filter dar.
237 236
 
238 237
 \begin{frame}
239 238
   \section{Kalman-Filter}
@@ -241,6 +240,11 @@
241 240
 \end{frame}
242 241
 
243 242
 \begin{frame}
  243
+  \section{Bewegunsgleichungen}
  244
+  \frametitle{Modelierung eines Systems}
  245
+\end{frame}
  246
+
  247
+\begin{frame}
244 248
   \section{Kalman-Filter für UAV}
245 249
   \frametitle{Kalman-Filter für UAV}
246 250
 \end{frame}

0 notes on commit e1bad1a

Please sign in to comment.
Something went wrong with that request. Please try again.