# publicmetaxy/5pk

### Subversion checkout URL

You can clone with HTTPS or Subversion.

some stuff

authored March 10, 2012

Showing 2 changed files with 190 additions and 62 deletions.

1. code/kreis_kalman.py
2. presentation/pre.tex
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   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   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   157 154   158 155   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   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   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   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   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   177 +  177 178  Für kleine Verschiebungen ist der Term $\Delta C x^{2}$ verschwindend klein. Es gilt also 178 179   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   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}