kalman filter fusion in matlab

Here is an example code for Kalman filter based sensor fusion in MATLAB. This code fuses measurements from a gyroscope and an accelerometer to estimate the orientation of an object in 3D space.

main.m
% Sensor Fusion using Kalman Filter
% Fusion of Gyroscope and Accelerometer measurements to estimate 3D orientation

% Define System Matrices
A = eye(6); % State Transition matrix
B = zeros(6, 3); % Control input matrix
H = [eye(3) zeros(3)]; % Observation matrix
Q = eye(6)*1e-6; % Process noise covariance matrix
R = eye(3)*0.1; % Measurement noise covariance matrix

% Initialize state vector and covariance matrix
x = zeros(6, 1); % State vector contains 3 angles and their first derivatives
P = eye(6);

% Load measurement data from sensors
gyroData = load('gyroData.mat');
accData = load('accData.mat');

% Extract sensor data and timestamps
gyro = gyroData.gyro;
acc = accData.acc;
tGyro = gyroData.t;
tAcc = accData.t;

% Initialize variables for storing estimates and timestamps
est = zeros(length(gyro), 3); % Estimated angles in degrees
tEst = zeros(length(gyro), 1); % Timestamps of estimates

% Kalman filter loop
for i = 2:length(gyro)
    % Calculate time delta between current and previous measurements
    dt = (tGyro(i) - tGyro(i-1))/1000;
    % Define state transition matrix based on time delta
    A(1:3, 4:6) = eye(3)*dt;

    % Predict state and covariance using system matrices
    x = A*x + B*gyro(i-1,:)';
    P = A*P*A' + Q;

    % Correct predicted state and covariance using measurement
    K = P*H'/(H*P*H' + R);
    z = acc(i,:)';
    y = z - H*x;
    x = x + K*y;
    P = (eye(6) - K*H)*P;

    % Store estimated angles and timestamps
    est(i, :) = x(1:3)';
    tEst(i) = tGyro(i);
end

% Plot estimated angles
plot(tEst, est)
xlabel('Time (ms)')
ylabel('Angle (degrees)')
legend({'Roll', 'Pitch', 'Yaw'})
1640 chars
57 lines

In this code, the state vector contains three angles and their first derivatives, and the observation matrix only measures the three angles from the accelerometer. The process noise covariance matrix and measurement noise covariance matrix are defined based on the expected noise characteristics of the sensors. The Kalman filter loop alternates between prediction and correction steps, where the predicted state and covariance are corrected based on the current measurement. The estimated angles are then stored and plotted.

gistlibby LogSnag