% This program is modified from file tilt.c by Trammell Hudson % for 1D tilt sensor using a dual axis % accelerometer and single axis angular rate gyro. The two sensors are % fused using Kalman filter. % Author: Ittichote Chuckpaiwong % Last updated: 20 Feb 2007 close all; clear; % Simulate signal with noise dt = 0.01; t = (0:dt:50)'; x = sin(t); % Angle (original) x_dot = cos(t); % Angular velocity v_gyro = sqrt(0.3)*randn(length(t),1); % measurement noise for gyro, variance = 0.3 gyro = x_dot + v_gyro; % Gyro measurement (pitch rate) v_acc = sqrt(0.3)*randn(length(t),1); % measurement noise for accellerometer, variance = 0.3 % Compute the angles computed by using only accelerometers of gyroscope x_acc = x + v_acc; % Angle computed from accelerometer measurement (pitch angle) x_gyro = cumsum(gyro*dt,1); % Angle computed by integration of gyro measurement % Plot the angles computed by using accelerometers or gyroscope alone figure; subplot(2,1,1); plot(t,[x_acc x_gyro x]); xlabel('time(s)'); ylabel('x(t)'); legend('accellerometer','gyro','actual'); % Following code is an implementation of Kalman filter. % It is modify from source code downloaded from rotomotion.com % which is originally written in C %/* % * Our covariance matrix. This is updated at every time step to % * determine how well the sensors are tracking the actual state. % */ P = [1 0; 0 1]; %/* % * R represents the measurement covariance noise. In this case, % * it is a 1x1 matrix that says that we expect 0.3 rad jitter % * from the accelerometer. % */ R_angle = 0.3; %/* % * Q is a 2x2 matrix that represents the process covariance noise. % * In this case, it indicates how much we trust the acceleromter % * relative to the gyros. % */ % Q_angle = 0.001; % Q_gyro = 0.003; Q_angle = 0.05; Q_gyro = 0.5; Q = [Q_angle 0; 0 Q_gyro]; %/* % * state_update is called every dt with a biased gyro measurement % * by the user of the module. It updates the current angle and % * rate estimate. % * % * The pitch gyro measurement should be scaled into real units, but % * does not need any bias removal. The filter will track the bias. % * % * Our state vector is: % * % * X = [ angle, gyro_bias ] % * % * It runs the state estimation forward via the state functions: % * % * Xdot = [ angle_dot, gyro_bias_dot ] % * % * angle_dot = gyro - gyro_bias % * gyro_bias_dot = 0 % * % * And updates the covariance matrix via the function: % * % * Pdot = A*P + P*A' + Q % * % * A is the Jacobian of Xdot with respect to the states: % * % * A = [ d(angle_dot)/d(angle) d(angle_dot)/d(gyro_bias) ] % * [ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ] % * % * = [ 0 -1 ] % * [ 0 0 ] % * % * Due to the small CPU available on the microcontroller, we've % * hand optimized the C code to only compute the terms that are % * explicitly non-zero, as well as expanded out the matrix math % * to be done in as few steps as possible. This does make it harder % * to read, debug and extend, but also allows us to do this with % * very little CPU time. % */ A = [0 -1; 0 0]; q_bias = 0; % Initialize gyro bias angle = 0; % Initialize gyro angle q_m = 0; X = [0; 0]; x1 = zeros(size(t)); x2 = zeros(size(t)); for i=1:length(t), % **************** Gyro update ******************* %q_m = q_m + gyro(i)*dt; % Compute pitch angle from gyro measurement q_m = gyro(i); q = q_m - q_bias; % /* Pitch gyro measurement */ % /* Unbias our gyro */ % const float q = q_m - q_bias; % % /* % * Compute the derivative of the covariance matrix % * % * Pdot = A*P + P*A' + Q % * % * We've hand computed the expansion of A = [ 0 -1, 0 0 ] multiplied % * by P and P multiplied by A' = [ 0 0, -1, 0 ]. This is then added % * to the diagonal elements of Q, which are Q_angle and Q_gyro. % */ Pdot = A*P + P*A' + Q; %/* Store our unbias gyro estimate */ rate = q; %/* % * Update our angle estimate % * angle += angle_dot * dt % * += (gyro - gyro_bias) * dt % * += q * dt % */ angle = angle + q*dt; %/* Update the covariance matrix */ P = P + Pdot*dt; % ************ Kalman (Accelerometer) update ***** % * The C matrix is a 1x2 (measurements x states) matrix that % * is the Jacobian matrix of the measurement value with respect % * to the states. In this case, C is: % * % * C = [ d(angle_m)/d(angle) d(angle_m)/d(gyro_bias) ] % * = [ 1 0 ] C = [1 0]; angle_err = x_acc(i)-angle; %/* % * Compute the error estimate. From the Kalman filter paper: % * % * E = C P C' + R % * % * Dimensionally, % * % * E<1,1> = C<1,2> P<2,2> C'<2,1> + R<1,1> % * % * Again, note that C_1 is zero, so we do not compute the term. % */ E = C*P*C' + R_angle; %/* % * Compute the Kalman filter gains. From the Kalman paper: % * % * K = P C' inv(E) % * % * Dimensionally: % * % * K<2,1> = P<2,2> C'<2,1> inv(E)<1,1> % * % * Luckilly, E is <1,1>, so the inverse of E is just 1/E. % */ K = P*C'*inv(E); % /* % * Update covariance matrix. Again, from the Kalman filter paper: % * % * P = P - K C P % * % * Dimensionally: % * % * P<2,2> -= K<2,1> C<1,2> P<2,2> % * % * We first compute t<1,2> = C P. Note that: % * % * t[0,0] = C[0,0] * P[0,0] + C[0,1] * P[1,0] % * % * But, since C_1 is zero, we have: % * % * t[0,0] = C[0,0] * P[0,0] = PCt[0,0] % * % * This saves us a floating point multiply. % */ P = P - K*C*P; % /* % * Update our state estimate. Again, from the Kalman paper: % * % * X += K * err % * % * And, dimensionally, % * % * X<2> = X<2> + K<2,1> * err<1,1> % * % * err is a measurement of the difference in the measured state % * and the estimate state. In our case, it is just the difference % * between the two accelerometer measured angle and our estimated % * angle. % */ X = X + K * angle_err; x1(i) = X(1); x2(i) = X(2); angle = x1(i); q_bias = x2(i); end % Plot the result using kalman filter subplot(2,1,2); plot(t,[x x1]); xlabel('time(s)'); ylabel('x(t)'); legend('actual','kalman');