| << Chapter < Page | Chapter >> Page > |
Kalman Filter
The Kalman filter is a time domain method of incorporating knowledge of the physical model of the system and of the reliability of the sensors to accurately estimate the state of the system. Implementation of the Kalman filter first requires the creation of an accurate physical model of the system. The two equations which are used to determine the estimate of the current state from that of the previous state are:
Near apogee, the physical equations governing the rocket’s flight are simple, which makes A simple. The only force acting on the rocket is gravity only (because drag forces vary with the square of the velocity they can be neglected near apogee, where the velocity is close to zero).
Where ∆t is the time between x k and x k+1 .
m is a vector of the measured values from the sensors. Position is measured with the barometer and acceleration by the accelerometer.
H is a matrix which maps x k to m:
Finally, K , the Kalman gain matrix, weights the difference between the measured values and the estimated values. K is typically computed in real time as the system changes. However, the formula for K is rather complicated and therefore difficult to implement on a microcontroller in real time. Luckily, because the rocket’s flight can be approximated over the whole flight by the system and because the sensor variances do not change, K can be precomputed via the following recursive process:
In a small number of repetitions, K will converge. In these equations, R is the measurement noise covariance matrix which holds the variances for each sensor:
P is called the error covariance matrix, and it is first approximated with a guess, and then recursively defined like the K matrix. Finally, Q is the process noise covariance matrix, and is associated with the amount of noise added to the estimate in each time step. The code for calculating the K matrix is shown below:
.% Calculates the Kalman gain
H = [1 0 0; 0 0 1]; % maps x (state variables) to z (sensor data)
R = [35.8229 0; 0 .0012]; % measurement noise covariance
Q = [0 0 0; 0 0 0; 0 0 1]; % process noise covariance matrix
T = .05; % time stepA = [1 T 1/2 * T^2; 0 1 T; 0 0 1]; % maps previous state to next state% these three equations recursively define k (matrix of kalman gains)
% and P (error covariance matrix)P = eye(3); % initial guess for p
for i = 1:20K = P*H'/(H*P*H' + R); % Kalman gainsP = (eye(3) - K *H)*P;
P = A*P*A' + Q;end
display(K)display(H)
display(P)
The last piece of code demonstrates the actual implementation of the Kalman filter in Matlab.
.% implements Kalman filter on altitude and accelerometer data. Required vectors are alt and accel, which are vectors cointaining the altitude and accelerometer data at times corresponding to the time vector t.
t = .05:.05:15;estimate = zeros(3,length(t));
estimate(:,1) = [alt(1); 0; accel(1)];
for i = 2:length(t)estimate(:,i) = A*estimate(:,i-1);
estimate(:,i) = estimate(:,i) + K*([alt(i);accel(i)]- H *estimate(:,i));end
Notification Switch
Would you like to follow the 'Elec 301 projects fall 2013' conversation and receive update notifications?
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|