Error State Kalman Filter Implementation (AHRS filter) issues

Hi, i am trying to implemente the error-state kalman filter described in here, i followed the equations described in the function documentation but the filter is not working properly, i have found some inconsistencies between the matlab function documentation and the original document, that might cause issue when coding the filter. Here is the code i am trying to implement.

classdef eskf < handle

    properties (SetAccess = private)

        sampleRate; % [Hz]
        accelNoise; % [(m/s^2)^2]
        magNoise; % [uT^2]
        gyroNoise; % [(rad/s)^2]
        gyroDrift; % [(rad/s)^2]
        linearAccelNoise; % [(m/s^2)^2]
        linearAccelDecay;
        magDisturbance; % [uT^2]
        magDisturbanceDecay; 
        initialProcessNoise;
        expectedMagFieldStrength; % [uT]

    end

    methods

        % Constructor
        function thiseskf = eskf(namedArgs)
            
            arguments
                namedArgs.sampleRate = 1024;
                namedArgs.accelNoise = 0.00019247;
                namedArgs.magNoise = 0.1;
                namedArgs.gyroNoise = 9.1385e-5;
                namedArgs.gyroDrift = 3.0462e-13;
                namedArgs.linearAccelNoise = 0.0096236;
                namedArgs.linearAccelDecay = 0.5;
                namedArgs.magDisturbance = 0.5;
                namedArgs.magDisturbanceDecay = 0.5;
                namedArgs.initialProcessNoise = diag([0.000006092348396*ones(1,3), ...
                    0.000076154354947*ones(1,3),0.00962361*ones(1,3), ...
                    0.6*ones(1,3)]);
                namedArgs.expectedMagFieldStrength = 50;
            end

            thiseskf.sampleRate = namedArgs.sampleRate;
            thiseskf.accelNoise = namedArgs.accelNoise;
            thiseskf.magNoise = namedArgs.magNoise;
            thiseskf.gyroNoise = namedArgs.gyroNoise;
            thiseskf.gyroDrift = namedArgs.gyroDrift;
            thiseskf.linearAccelNoise = namedArgs.linearAccelNoise;
            thiseskf.linearAccelDecay = namedArgs.linearAccelDecay;
            thiseskf.magDisturbance = namedArgs.magDisturbance;
            thiseskf.magDisturbanceDecay = namedArgs.magDisturbanceDecay;
            thiseskf.initialProcessNoise = namedArgs.initialProcessNoise;
            thiseskf.expectedMagFieldStrength = namedArgs.expectedMagFieldStrength;

        end

        % Run ESKF
        function [orientation,angularVelocity] = runESKF(thiseskf,accel,gyro,mag)

            % Initialize variables
            qPrior = ecompassFunc(thiseskf,accel(1,:),mag(1,:));
            pPrior = thiseskf.initialProcessNoise;
            gyroOffset = zeros(1,3);
            m = [thiseskf.expectedMagFieldStrength,0,0];
            linAccelPrior = zeros(1,3);
            magJamming = false;
        
            for i = 1 : size(accel,1)

                [gAccel,gGyro,mGyro] = sensorModel(thiseskf,accel(i,:),gyro(i,:),qPrior,m,linAccelPrior,gyroOffset);

                z = errorModel(thiseskf,gAccel/9.81,gGyro,mag(i,:),mGyro);
               
                [k,x,pPrior] = kalmanEquations(thiseskf,z,mGyro,gGyro,magJamming,pPrior);

                [qPrior,linAccelPrior,gyroOffset] = correct(thiseskf,x,qPrior,linAccelPrior,gyroOffset);
                
                orientation(i,:) = qPrior;

                angularVelocity(i,:) = calAngularVelocity(thiseskf,gyro(i,:),gyroOffset);
                
                [m,magJamming] = magCorrect(thiseskf,z,k,quat2rotm(qPrior),m);
                    
            end            

        end

    end

    methods (Access = private)

        % Sensors Model
        function [gAccel,gGyro,mGyro] = sensorModel(thiseskf,accel,gyro,qPrior,m,linAccelPrior,gyroOffset)

            wdt = (gyro - gyroOffset)/thiseskf.sampleRate;

            q = quatmultiply(qPrior,quatrotvec(thiseskf,wdt));
            
            R = quat2rotm(q);
            gGyro = R(:,3)';
            
            gAccel = accel - linAccelPrior;

            mGyro = (R*m')';
            
        end

        % Error Model
        function z = errorModel(thiseskf,gAccel,gGyro,mMag,mGyro)
            
            z = [gGyro-gAccel,mGyro-mMag];
            z(1:3) = z(1:3)*9.81;
      
        end

        % Kalman Equations
        function [k,x,Qw] = kalmanEquations(thiseskf,z,mGyro,gGyro,magJamming,pPrior)

            dt = 1/thiseskf.sampleRate;

            % Measurement Matrix
            H = [0,gGyro(3),-gGyro(2),0,-dt*gGyro(3),dt*gGyro(2),1,0,0,0,0,0;
                -gGyro(3),0,gGyro(1),dt*gGyro(3),0,-dt*gGyro(1),0,1,0,0,0,0;
                gGyro(2),-gGyro(1),0,-dt*gGyro(2),dt*gGyro(1),0,0,0,1,0,0,0;
                0,mGyro(3),-mGyro(2),0,-dt*mGyro(3),-dt*mGyro(2),0,0,0,-1,0,0;
                -mGyro(3),0,mGyro(1),dt*mGyro(3),0,-dt*mGyro(1),0,0,0,0,-1,0;
                mGyro(2),-mGyro(1),0,-dt*mGyro(2),dt*mGyro(1),0,0,0,0,0,0,-1];

            % Measurement Noise Covariance
            accelNoise = thiseskf.accelNoise + thiseskf.linearAccelNoise + ...
                dt^2*(thiseskf.gyroDrift + thiseskf.gyroNoise);
            magNoise = thiseskf.magNoise + thiseskf.magDisturbance + ...
                dt^2*(thiseskf.gyroDrift + thiseskf.gyroNoise);

            Qv = [accelNoise,0,0,0,0,0;
                0,accelNoise,0,0,0,0;
                0,0,accelNoise,0,0,0;
                0,0,0,magNoise,0,0;
                0,0,0,0,magNoise,0;
                0,0,0,0,0,magNoise];

            S = (H*pPrior*H' + Qv)';
            
            k = pPrior*H'/S;

            P = pPrior - k*H*pPrior;

            Qw = [P(1)+dt^2*P(40)+thiseskf.gyroDrift+thiseskf.gyroNoise,0,0,-dt*(P(40)+thiseskf.gyroDrift),0,0,0,0,0,0,0,0;
                0,P(14)+dt^2*P(53)+thiseskf.gyroDrift+thiseskf.gyroNoise,0,0,-dt*(P(53)+thiseskf.gyroDrift),0,0,0,0,0,0,0;
                0,0,P(27)+dt^2*P(66)+thiseskf.gyroDrift+thiseskf.gyroNoise,0,0,-dt*(P(66)+thiseskf.gyroDrift),0,0,0,0,0,0;
                -dt*(P(40)+thiseskf.gyroDrift),0,0,P(40)+thiseskf.gyroDrift,0,0,0,0,0,0,0,0;
                0,-dt*(P(53)+thiseskf.gyroDrift),0,0,P(53)+thiseskf.gyroDrift,0,0,0,0,0,0,0;
                0,0,-dt*(P(66)+thiseskf.gyroDrift),0,0,P(66)+thiseskf.gyroDrift,0,0,0,0,0,0;
                0,0,0,0,0,0,thiseskf.linearAccelDecay^2*P(79)+thiseskf.linearAccelNoise,0,0,0,0,0;
                0,0,0,0,0,0,0,thiseskf.linearAccelDecay^2*P(92)+thiseskf.linearAccelNoise,0,0,0,0;
                0,0,0,0,0,0,0,0,thiseskf.linearAccelDecay^2*P(105)+thiseskf.linearAccelNoise,0,0,0;
                0,0,0,0,0,0,0,0,0,thiseskf.magDisturbanceDecay^2*P(118)+thiseskf.magDisturbance,0,0;
                0,0,0,0,0,0,0,0,0,0,thiseskf.magDisturbanceDecay^2*P(131)+thiseskf.magDisturbance,0;
                0,0,0,0,0,0,0,0,0,0,0,thiseskf.magDisturbanceDecay^2*P(144)+thiseskf.magDisturbance];

            if ~magJamming
                x = k*z';
            else
                x = k(1:9,1:3)*z(1:3)';
            end

        end

        % Mag Correct
        function [m,magJamming] = magCorrect(thiseskf,z,k,R,m)

            mError = (k(10:12,:)*z')';

            if sum(mError.^2) > 4*thiseskf.expectedMagFieldStrength^2
                magJamming = true;               
            else
                magJamming = false;

                % Update Magnetic Vector
                mErrorNED = (R'*mError')';
                
                M = m - mErrorNED;
    
                inclination = atan2(M(3),M(1));
    
                m = [thiseskf.expectedMagFieldStrength*cos(inclination),0,...
                    thiseskf.expectedMagFieldStrength*sin(inclination)];
            end            

        end

        % Correct
        function [q,linAccelPrior,gyroOffset] = correct(thiseskf,x,qPrior,linAccelPrior_k_1,gyroOffsetPrior)

            q = quatmultiply(qPrior,quatrotvec(thiseskf,-x(1:3)));
            linAccelPrior = linAccelPrior_k_1*thiseskf.linearAccelDecay - x(7:9)';            
            gyroOffset = gyroOffsetPrior - x(4:6)';

        end

        % Angular Velocity
        function angularVelocity = calAngularVelocity(thiseskf,gyro,gyroOffset)  
            angularVelocity = gyro - gyroOffset;            
        end

        % Rotation vector to quaternion
        function q = quatrotvec(thiseskf,wdt)
            R = [1,wdt(3),-wdt(2);-wdt(3),1,wdt(1);wdt(2),-wdt(1),1];
            q = rotm2quat(R);
        end

        % Orientation from accelerometer and magnetometer
        function q = ecompassFunc(thiseskf,accel,mag)
            R = [cross(cross(accel,mag),accel)',cross(accel,mag)',accel']; 
            q = rotm2quat(R');            
        end

    end
end

The file ahrsdata.mat contains the data i am using to test the filter, the data was acquired at 1024 Hz and is structered as follows: Accel X - Accel Y - Accel Z - Gyro X - Gyro Y - Gyro Z - Mag X - Mag Y - Mag Z. Accel readings are in , Gyro readings are in and Mag readings are in . I am comparing my implementation with the ahrsfilter matlab function. Any help would be appreciated.

Thanks,