Kalman: Unterschied zwischen den Versionen
Aus www.wiki.ardumower.de
(→Idee) |
(→Beispiel) |
||
| Zeile 11: | Zeile 11: | ||
# Correct: we correct the certaincy based on new measurements for each sensor. Does the prediction fit to the sensor measurement, we increase certaincy for that sensor, otherwise we decrease certaincy. | # Correct: we correct the certaincy based on new measurements for each sensor. Does the prediction fit to the sensor measurement, we increase certaincy for that sensor, otherwise we decrease certaincy. | ||
| − | = | + | =Example= |
| − | + | We have a heading (theta) and a heading speed rate (omega). The new heading can be predicted by old heading plus heading rate and delta time: | |
theta = theta_k+1 + omega * dt | theta = theta_k+1 + omega * dt | ||
| − | theta_k+1: | + | theta_k+1: new heading |
| − | theta_k: | + | theta_k: old heading |
| − | omega: | + | omega: heading speed rate |
| − | dt: | + | dt: delta time |
| − | + | The heading speed rate is modelled after this: | |
omega = omega_k+1 | omega = omega_k+1 | ||
| − | + | This is a model only. The real heading rate will change. The Kalman considers this. Those both state equations can now be turned into state space form which is basically a matrix form of the two equations: | |
x_k = A * x_k+1 | x_k = A * x_k+1 | ||
| − | + | with | |
x = [ theta | x = [ theta | ||
| Zeile 38: | Zeile 38: | ||
0 1 ] | 0 1 ] | ||
| − | + | If you muliply the matrix equation, you get again both state equations. Kalman filter also uses additional matrizes: | |
y_k = C * yk+1 | y_k = C * yk+1 | ||
| − | + | with | |
y = [ theta | y = [ theta | ||
| Zeile 50: | Zeile 50: | ||
0 1 ] | 0 1 ] | ||
| − | + | Matrix C transfers measurements into state variables. The Kalman filter also uses Covariance Matrix P which describes how well state variables and measurements fit. | |
| − | + | Furthermore, Kalman uses a measurement error matrix R where you can estimate the measurement error for each signal. | |
| − | + | Finally, there's a process error matrix Q which models the complete system error (due to noise in servos, motors etc). | |
=Scilab= | =Scilab= | ||
Version vom 27. Februar 2015, 15:11 Uhr
Idea
You have...
- A description of a robots state at every time (e.g. actual heading, actual heading speed).
- A certain number of sensors. Each sensor has a certain measurement error (%).
Kalman fusions all sensors measurements by iterating over and over two phases:
- Predict: we predict the next robot's state by help of the old robot's state and a certaincy for each sensor.
- Correct: we correct the certaincy based on new measurements for each sensor. Does the prediction fit to the sensor measurement, we increase certaincy for that sensor, otherwise we decrease certaincy.
Example
We have a heading (theta) and a heading speed rate (omega). The new heading can be predicted by old heading plus heading rate and delta time:
theta = theta_k+1 + omega * dt
theta_k+1: new heading theta_k: old heading omega: heading speed rate dt: delta time
The heading speed rate is modelled after this:
omega = omega_k+1
This is a model only. The real heading rate will change. The Kalman considers this. Those both state equations can now be turned into state space form which is basically a matrix form of the two equations:
x_k = A * x_k+1
with
x = [ theta
omega ]
A = [ 1 dt
0 1 ]
If you muliply the matrix equation, you get again both state equations. Kalman filter also uses additional matrizes:
y_k = C * yk+1
with
y = [ theta
omega ]
C = [ 1 0
0 1 ]
Matrix C transfers measurements into state variables. The Kalman filter also uses Covariance Matrix P which describes how well state variables and measurements fit.
Furthermore, Kalman uses a measurement error matrix R where you can estimate the measurement error for each signal.
Finally, there's a process error matrix Q which models the complete system error (due to noise in servos, motors etc).
Scilab
Beschreibung des Algorithmus mit Hilfe des kostenloen Mathematik-Paketes "Scilab":
% Initialisierung A=[1 dt; 0 1]; % Zustandsübergangsmatrix - gibt an wie wir vom aktuellen zum nächsten Zustand kommen C=[1 0; 0 1]; % Matrix bildet Mess-Zustand auf System-Zustand ab P=[1000 0; 0 1000]; % Start-Kovarianz für anfängliche Unsicherheit Q=[0.01 0; 0 0.01]; % Prozess-Fehler; Zum Anpassen/Spielen R=[0.01 0; 0 0.01]; % Messfehler; Zum Anpassen/Spielen
% Durchlauf das folgende für jede neue Messung
% VORHERSAGE % wir sagen den nächsten System-Zustand vorher basierend of dem Wissen (Modell) unseres Systems x = A*x; % Wir passen auch die Unsicherheit an. Wenn wir den Systemzustand ohne Messungen vorhersagen, steigt die Unsicherheit P = A*P*A' + Q; % Unsicherheit auch mit Zustandsübergang anpassen. Prozess-Fehler addieren % KORREKTUR % Mit den Messungen korrigieren wir die Zustands-Schätzung z=[gpsHdg; gyroHdgRate]; % Dies ist die Mess-Matrix % Zunächst Kalman-Verstärkung herausfinden; wie stark vertrauen wir der Schätzung im Vergleich zu den Messungen K = P*C'*inv(C*P*C' + R) % Dann den Fehler zwischen Vorhersage und Messungen herausfinden (die "Innovation") % z-C*x % und korrigiere die Schätzung -- aber nur ein kleines bisschen pro Zeiteinheit, % bestimmt durch die Kalman-Verstärkung x = x + K*(z-C*x) % Genauso: korrigiere (genauer: erniedrige) Unsicherheit da wir nach jeder Messung ein kleines bisschen % mehr Sicher sein können über unsere Schätzung P = (I-K*C)*P
