kalman filter fusion in matlab

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