### Implement Extended Kalman Filter for Attitude Estimation

Source: https://context7.com/rflysim/rflyexpcode/llms.txt

The function fuses gyroscope and accelerometer data to estimate state and Euler angles. The example loop demonstrates how to initialize the filter and process sequential measurement data.

```matlab
function [x_aposteriori, P_aposteriori, phi, theta] = Attitude_ekf(dt, z, q, r, x_aposteriori_k, P_aposteriori_k)
% Extended Kalman Filter for attitude state estimation
% Inputs:
%   dt: Sampling time (s)
%   z: Measurement data [wx, wy, wz, ax, ay, az]' (rad/s, m/s^2)
%   q: Process noise covariance [q_gyro, q_accel]
%   r: Measurement noise covariance
%   x_aposteriori_k: Previous state estimate [bx, by, bz, kx, ky, kz]' (6x1)
%   P_aposteriori_k: Previous error covariance (6x6)
% Outputs:
%   x_aposteriori: Updated state estimate (6x1)
%   P_aposteriori: Updated error covariance (6x6)
%   phi: Roll angle (rad)
%   theta: Pitch angle (rad)

    w_m = z(1:3);  % Angular velocity measurement
    a_m = z(4:6);  % Acceleration measurement
    g = norm(a_m); % Gravity magnitude

    % Build rotation matrix from angular velocity
    w_x_ = [0, -(w_m(3)-x_aposteriori_k(3)), w_m(2)-x_aposteriori_k(2);
            w_m(3)-x_aposteriori_k(3), 0, -(w_m(1)-x_aposteriori_k(1));
            -(w_m(2)-x_aposteriori_k(2)), w_m(1)-x_aposteriori_k(1), 0];
    bCn = eye(3) - w_x_*dt;

    % Prediction step
    x_apriori = zeros(6, 1);
    x_apriori(1:3) = x_aposteriori_k(1:3);      % Gyro bias (constant)
    x_apriori(4:6) = bCn * x_aposteriori_k(4:6); % Normalized acceleration

    % State transition and process noise matrices
    x_k_x = [0, -x_aposteriori_k(6), x_aposteriori_k(5);
             x_aposteriori_k(6), 0, -x_aposteriori_k(4);
             -x_aposteriori_k(5), x_aposteriori_k(4), 0];
    PHI = [eye(3), zeros(3); -x_k_x*dt, bCn];
    GAMMA = [eye(3)*dt, zeros(3); zeros(3), -x_k_x*dt];
    Q = blkdiag(eye(3)*q(1), eye(3)*q(2));
    P_apriori = PHI * P_aposteriori_k * PHI' + GAMMA * Q * GAMMA';

    % Update step
    R = eye(3) * r(1);
    H_k = [zeros(3), -g*eye(3)];
    K_k = (P_apriori * H_k') / (H_k * P_apriori * H_k' + R);
    x_aposteriori = x_apriori + K_k * (a_m - H_k * x_apriori);
    P_aposteriori = (eye(6) - K_k * H_k) * P_apriori;

    % Extract Euler angles
    k = x_aposteriori(4:6) / norm(x_aposteriori(4:6));
    phi = atan2(k(2), k(3));
    theta = -asin(k(1));
end

% Example: Initialize and run EKF
dt = 0.004;
q = [0.001, 0.01];       % Process noise
r = [0.1];               % Measurement noise
x = [0; 0; 0; 0; 0; 1];  % Initial state (gravity along z)
P = eye(6) * 0.1;        % Initial covariance

for k = 1:numSamples
    z = [gyro(:,k); accel(:,k)];
    [x, P, roll, pitch] = Attitude_ekf(dt, z, q, r, x, P);
    roll_history(k) = roll * 180/pi;
    pitch_history(k) = pitch * 180/pi;
end
```

--------------------------------

### Initialize UAV Model Parameters in MATLAB

Source: https://context7.com/rflysim/rflyexpcode/llms.txt

This script initializes all necessary UAV physical parameters, control gains, and environmental settings for simulation. Run this before launching any Simulink models.

```matlab
% UAV model parameter initialization
% Run this script before launching any Simulink model

% Constant parameters
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;

% Initial conditions
ModelInit_PosE = [0, 0, -100];      % Initial position [x, y, z] in NED frame (m)
ModelInit_VelB = [0, 0, 0];         % Initial velocity [u, v, w] in body frame (m/s)
ModelInit_AngEuler = [0, 0, 0];     % Initial Euler angle [roll, pitch, yaw] (rad)
ModelInit_RateB = [0, 0, 0];        % Initial angular velocity [p, q, r] (rad/s)
ModelInit_Rads = 557.1420;          % Initial motor speed (rad/s)

% UAV physical parameters
ModelParam_uavMass = 1.4;           % Mass of UAV (kg)
ModelParam_uavJxx = 0.0211;         % Moment of inertia about x-axis (kg.m^2)
ModelParam_uavJyy = 0.0219;         % Moment of inertia about y-axis (kg.m^2)
ModelParam_uavJzz = 0.0366;         % Moment of inertia about z-axis (kg.m^2)
ModelParam_uavJ = diag([ModelParam_uavJxx, ModelParam_uavJyy, ModelParam_uavJzz]);
ModelParam_uavR = 0.225;            % Body radius (m)
ModelParam_uavType = int8(3);       % X-type quadrotor
ModelParam_uavMotNumbs = int8(4);   % Number of motors

% Motor parameters
ModelParam_motorCr = 1148;          % Motor throttle-speed curve slope (rad/s)
ModelParam_motorWb = -141.4;        % Motor speed-throttle curve constant (rad/s)
ModelParam_motorT = 0.02;           % Motor inertia time constant (s)
ModelParam_motorJm = 0.0001287;     % Moment of inertia of motor + propeller (kg.m^2)

% Rotor aerodynamic coefficients: M = Cm*w^2, T = Ct*w^2
ModelParam_rotorCm = 1.779e-07;     % Rotor torque coefficient (kg.m^2)
ModelParam_rotorCt = 1.105e-05;     % Rotor thrust coefficient (kg.m^2)

% Calculate hovering throttle
THR_HOVER = (sqrt(ModelParam_uavMass*9.8/4/ModelParam_rotorCt) - ModelParam_motorWb)/ModelParam_motorCr;

% Environment parameters
ModelParam_envGravityAcc = 9.8;                  % Gravity acceleration (m/s^2)
ModelParam_envLongitude = 116.259368300000;      % Reference longitude (deg)
ModelParam_envLatitude = 40.1540302;             % Reference latitude (deg)
ModelParam_GPSLatLong = [ModelParam_envLatitude ModelParam_envLongitude];
ModelParam_BusSampleRate = 0.001;                % Model sampling rate (s)

```

--------------------------------

### Configure Attitude Controller Parameters

Source: https://context7.com/rflysim/rflyexpcode/llms.txt

This script initializes PID gains, anti-windup limits, and safety constraints for UAV attitude control. It assumes the existence of an 'Init' function to load model parameters.

```matlab
% Attitude Controller Configuration (e5.1)
clear
path('./icon/', path);
Init;  % Load UAV model parameters

% Constants
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
THR_HOVER = 0.609;  % Hovering throttle (normalized)

% Initial conditions
ModelInit_PosE = [0, 0, 0];
ModelInit_VelB = [0, 0, 0];
ModelInit_AngEuler = [0, 0, 0];
ModelInit_RateB = [0, 0, 0];
ModelInit_Rads = 0;

% Pitch axis PID parameters (cascaded: angle -> angle rate)
Kp_PITCH_ANGLE = 6.5;        % Outer loop P gain
Kp_PITCH_AngleRate = 0.1;    % Inner loop P gain
Ki_PITCH_AngleRate = 0.02;   % Inner loop I gain
Kd_PITCH_AngleRate = 0.001;  % Inner loop D gain

% Roll axis PID parameters
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;

% Yaw axis PID parameters (rate control only)
Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;

% Anti-windup: integral saturation limits
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;

% Safety limits
MAX_CONTROL_ANGLE_ROLL = 35;       % Maximum roll angle (deg)
MAX_CONTROL_ANGLE_PITCH = 35;      % Maximum pitch angle (deg)
MAX_CONTROL_ANGLE_RATE_PITCH = 220; % Maximum pitch rate (deg/s)
MAX_CONTROL_ANGLE_RATE_ROLL = 220;  % Maximum roll rate (deg/s)
MAX_CONTROL_ANGLE_RATE_Y = 200;     % Maximum yaw rate (deg/s)

% Run Simulink model
AttitudeControl_Sim
```

--------------------------------

### Configure Failsafe Logic Controller Parameters

Source: https://context7.com/rflysim/rflyexpcode/llms.txt

Initializes PID gains, saturation limits, and navigation safety constraints for the e8.1 failsafe experiment. Requires the Init function and associated icon path to be present in the environment.

```matlab
% Failsafe Logic Controller Configuration (e8.1)
clear
path('./icon/', path);
Init;

RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
THR_HOVER = 0.609;

% Attitude PID parameters
Kp_PITCH_ANGLE = 6.5;
Kp_PITCH_AngleRate = 0.1;
Ki_PITCH_AngleRate = 0.02;
Kd_PITCH_AngleRate = 0.001;
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;
Kp_YAW_ANGLE = 3;  % Yaw angle proportional gain for heading hold
Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;

% Position PID parameters
Kpxp = 1.0; Kpyp = 1.0; Kpzp = 4.0;
Kvxp = 3.0; Kvxi = 0.1; Kvxd = 0.01;
Kvyp = 3.0; Kvyi = 0.1; Kvyd = 0.01;
Kvzp = 0.45; Kvzi = 0.01; Kvzd = 0.005;

% Saturation limits
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;
Saturation_I_az = 5;

% Standard control limits
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH = 35;
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
MAX_CONTROL_VELOCITY_XY = 5;
MAX_CONTROL_VELOCITY_Z = 3;
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;

% Navigation safety limits (more conservative for failsafe mode)
MAX_CONTROL_ANGLE_NAV_ROLL = 15;   % Reduced roll limit during navigation
MAX_CONTROL_ANGLE_NAV_PITCH = 15;  % Reduced pitch limit during navigation

% Run Simulink model
e8_1_sim
```

--------------------------------

### Initialize Position Controller

Source: https://context7.com/rflysim/rflyexpcode/llms.txt

Sets up PID gains for inner-loop attitude and outer-loop position control, along with safety limits and integral saturation bounds. Executes the PosControl_Sim model upon completion.

```matlab
% Position Controller Configuration (e6.1)
clear
path('./icon/', path);
Init;

RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
THR_HOVER = 0.609;

% Attitude PID parameters (inner loop)
Kp_PITCH_ANGLE = 6.5;
Kp_PITCH_AngleRate = 0.1;
Ki_PITCH_AngleRate = 0.02;
Kd_PITCH_AngleRate = 0.001;
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;
Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;

% Position PID parameters (outer loop)
% Horizontal position (X, Y) - P controller
Kpxp = 1.0;  % Position X proportional gain
Kpyp = 1.0;  % Position Y proportional gain
Kpzp = 4.0;  % Position Z proportional gain (altitude)

% Velocity control - PID controller
Kvxp = 2.5; Kvxi = 0.4; Kvxd = 0.01;  % Velocity X gains
Kvyp = 2.5; Kvyi = 0.4; Kvyd = 0.01;  % Velocity Y gains
Kvzp = 0.45; Kvzi = 0.01; Kvzd = 0.005;  % Velocity Z gains

% Integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;  % Horizontal acceleration integral limit
Saturation_I_az = 5;     % Vertical acceleration integral limit

% Safety limits
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH = 35;
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
MAX_CONTROL_VELOCITY_XY = 5;   % Maximum horizontal speed (m/s)
MAX_CONTROL_VELOCITY_Z = 3;    % Maximum vertical speed (m/s)
MAX_MAN_THR = 0.9;             % Maximum throttle
MIN_MAN_THR = 0.05;            % Minimum throttle

% Run Simulink model
PosControl_Sim
```

--------------------------------

### Levenberg-Marquardt Algorithm for Nonlinear Least Squares

Source: https://context7.com/rflysim/rflyexpcode/llms.txt

Implements the Levenberg-Marquardt algorithm for sensor calibration and parameter estimation. Requires initial parameter estimates, input data, and optionally step size and parameter bounds.

```matlab
function p = lm(func, p, x, y_dat, dp, p_min, p_max)
% Levenberg-Marquardt algorithm for nonlinear least squares
% Inputs:
%   func: Function handle, y_hat = func(x, p)
%   p: Initial parameter estimates (n x 1)
%   x: Input data
%   y_dat: Desired output values (m x 1)
%   dp: Step size for Jacobian computation (default: 0.001)
%   p_min, p_max: Parameter bounds (default: -/+100*|p|)
% Output:
%   p: Optimized parameters

    p = p(:); y_dat = y_dat(:);
    Npar = length(p);

    % Algorithm parameters
    MaxIter = 200000 * Npar;
    epsilon_1 = 1e-6;   % Gradient convergence tolerance
    epsilon_2 = 1e-6;   % Parameter convergence tolerance
    epsilon_4 = 1e-6;   % Step acceptance threshold
    lambda_0 = 1e-5;    % Initial damping factor

    % Set defaults
    if nargin < 5, dp = 0.001; end
    if nargin < 6, p_min = -100*abs(p); end
    if nargin < 7, p_max = 100*abs(p); end

    % Initialize
    [alpha, beta, delta_y] = lm_matx(func, x, p, y_dat, dp);
    Fx_old = delta_y' * delta_y;
    lambda = lambda_0 * max(diag(alpha));
    nu = 2;

    iteration = 0;
    while iteration <= MaxIter
        iteration = iteration + 1;

        % Compute parameter update
        delta_p = (alpha + lambda*diag(diag(alpha))) \ beta;
        p_try = min(max(p_min, p + delta_p), p_max);

        % Evaluate new residual
        delta_y = y_dat - feval(func, x, p_try)';
        Fx = delta_y' * delta_y;
        rho = (Fx_old - Fx) / (delta_p' * (lambda*delta_p + beta));

        if rho > epsilon_4
            p = p_try;
            [alpha, beta, delta_y] = lm_matx(func, x, p, y_dat, dp);
            Fx_old = delta_y' * delta_y;
            lambda = lambda * max(1/3, 1-(2*rho-1)^3);
            nu = 2;
        else
            lambda = lambda * nu;
            nu = 2 * nu;
        end

        % Check convergence
        if max(abs(delta_p./p)) < epsilon_2 || max(abs(beta)) < epsilon_1
            break;
        end
    end
end
```

```matlab
% Example: Calibrate accelerometer
load('AccRaw.mat');
p_init = [1 1 1 0 0 0]';
p_fit = lm('calFunc', p_init, AccRaw, 9.8*ones(size(AccRaw,2),1));
fprintf('Scale factors: [%.4f, %.4f, %.4f]\n', p_fit(1:3));
fprintf('Bias offsets:  [%.4f, %.4f, %.4f]\n', p_fit(4:6));
```

--------------------------------

### Initialize ADRC Attitude Controller

Source: https://context7.com/rflysim/rflyexpcode/llms.txt

Configures ADRC parameters including TD, NLSEF, and ESO gains for attitude control. Includes disturbance injection settings and executes the AttitudeControl_ADRC_Sim model.

```matlab
% ADRC Attitude Controller Configuration
clear
path('./icon/', path);
Init;

% Disturbance injection for testing robustness
DisturbOn = 0;           % 0: off, 1: on
DisturbStartTime = 2.0;  % Disturbance start time (s)

RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
THR_HOVER = 0.609;

% Initial conditions
ModelInit_PosE = [0, 0, 0];
ModelInit_VelB = [0, 0, 0];
ModelInit_AngEuler = [0, 0, 0];
ModelInit_RateB = [0, 0, 0];
ModelInit_Rads = 0;

% Yaw axis still uses PID
Kp_YAW_AngleRate = 0.5;
Ki_YAW_AngleRate = 0.01;
Kd_YAW_AngleRate = 0.00;

% Integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;

% Safety limits
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH = 35;
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;

% ADRC parameters
% Tracking Differentiator (TD)
SL_TD_R = single(20.0);    % TD tracking speed
SL_TD_H = single(1.0);     % TD filter factor

% Nonlinear State Error Feedback (NLSEF)
SL_NLSEF_R = single(200.0);  % NLSEF gain
SL_NLSEF_H = single(25.0);   % NLSEF smoothing factor
SL_NLSEF_C = single(5.0);    % NLSEF compensation factor

% Extended State Observer (ESO)
SL_ESO_B01 = single(250.0);   % ESO bandwidth parameter 1
SL_ESO_B02 = single(2000.0);  % ESO bandwidth parameter 2
SL_ESO_B03 = single(3000.0);  % ESO bandwidth parameter 3
SL_ESO_B0 = single(47.4);     % ESO gain
SL_ESO_H = single(1.0);       % ESO filter factor
SL_ESO_D = single(0.01);      % ESO dead zone

% Torque to sigma conversion
tau2sigma = single(0.108);

% RC input calibration
RC1_MIN = single(1050); RC1_MAX = single(1950);
RC2_MIN = single(1050); RC2_MAX = single(1950);
RC3_MIN = single(1050); RC3_MAX = single(1950);
RC4_MIN = single(1050); RC4_MAX = single(1950);

% Run Simulink model
AttitudeControl_ADRC_Sim
```

--------------------------------

### Add Custom Topic to Logger Configuration

Source: https://github.com/rflysim/rflyexpcode/blob/master/code/e5/e5.4/PSPfile/README.txt

Register the custom message topic with its logging period in the logger configuration file. For PX4 versions 1.11 and above, use 'logged_topics.cpp'; otherwise, use 'logger.cpp'. The second argument specifies the logging period in milliseconds.

```c++
add_topic( "costom_attctrl_e5", 4);

```

--------------------------------

### Add Custom Message to CMakeLists.txt

Source: https://github.com/rflysim/rflyexpcode/blob/master/code/e5/e5.4/PSPfile/README.txt

Append the custom message file name to the list of message files in the Firmware/msg/CmakeLists.txt. Ensure this is done after the 'set(msg_files' command.

```cmake
costom_attctrl_e5.msg
```

--------------------------------

### Read PX4 Binary Log File

Source: https://context7.com/rflysim/rflyexpcode/llms.txt

Reads data from PX4 binary log files. Parses header information and extracts sensor data. Ensure the file path is correct.

```matlab
function [datapts, numpts] = px4_read_binary_file(dataFile)
% Read data from a PX4 binary log file
% Usage: [datapoints, numpoints] = px4_read_binary_file('datafile.bin')
% 
% Header format:
%   String "MWLOGV##" (8 bytes)
%   Time/Date (4 bytes, uint32)
%   Number of signals per record (1 byte, max 256)
%   Data type of signals (1 byte, 1-10)
%   Number of bytes per record (2 bytes, max 65535)

    if nargin == 0
        dataFile = 'data.bin';
    end

    fid = fopen(dataFile, 'r');
    hdrToken = fread(fid, 8, 'char');

    if strncmp(char(hdrToken), 'MWLOGV', 6) == true
        logTime = uint32(fread(fid, 1, 'uint32'));
        numflds = double(fread(fid, 1, 'uint8'));
        typefld = uint8(fread(fid, 1, 'uint8'));
        recSize = uint16(fread(fid, 1, 'uint16'));
        fieldTypeStr = get_elem_type(typefld);
        datapts = fread(fid, double([numflds, Inf]), fieldTypeStr);
        fclose(fid);
        numpts = size(datapts, 2);
    end
end
```

```matlab
% Example usage:
% [datapoints, numpoints] = px4_read_binary_file('flight_log.bin');
% ax = datapoints(1, :);  % Accelerometer X (m/s^2)
% ay = datapoints(2, :);  % Accelerometer Y (m/s^2)
% az = datapoints(3, :);  % Accelerometer Z (m/s^2)
% gx = datapoints(4, :);  % Gyroscope X (rad/s)
% gy = datapoints(5, :);  % Gyroscope Y (rad/s)
% gz = datapoints(6, :);  % Gyroscope Z (rad/s)
% phi = datapoints(7, :); % Roll angle from PX4 (rad)
% theta = datapoints(8, :); % Pitch angle from PX4 (rad)
% timestamp = datapoints(9, :); % Timestamp (us)
```

--------------------------------

### Complementary Filter for Attitude Estimation

Source: https://context7.com/rflysim/rflyexpcode/llms.txt

Implements a complementary filter to fuse gyroscope and accelerometer data for roll and pitch angle estimation. It requires sampling time, sensor data, previous angle estimates, and a filter time constant.

```matlab
function [phi_cf, theta_cf] = Attitude_cf(dt, z, phi_cf_k, theta_cf_k, tao)
% Complementary filter for attitude estimation
% Inputs:
%   dt: Sampling time (s)
%   z: Sensor data [gx, gy, gz, ax, ay, az]' (rad/s, m/s^2)
%   phi_cf_k: Previous roll estimate (rad)
%   theta_cf_k: Previous pitch estimate (rad)
%   tao: Filter time constant (s), typical value: 0.1
% Outputs:
%   phi_cf: Current roll angle estimate (rad)
%   theta_cf: Current pitch angle estimate (rad)

    gx = z(1); gy = z(2);          % Gyroscope data
    ax = z(4); ay = z(5); az = z(6); % Accelerometer data

    % Calculate Euler angles from accelerometer
    g = sqrt(ax^2 + ay^2 + az^2);
    theta_am = asin(ax/g);
    phi_am = -asin(ay/(g*cos(theta_am)));

    % Complementary filter: blend gyro integration with accelerometer
    alpha = tao / (tao + dt);  % High-pass weight (gyro)
    beta = dt / (tao + dt);    % Low-pass weight (accel)

    theta_cf = alpha*(theta_cf_k + gy*dt) + beta*theta_am;
    phi_cf = alpha*(phi_cf_k + gx*dt) + beta*phi_am;
end
```

```matlab
% Example: Process sensor data stream
dt = 0.004;      % 250 Hz sampling
tao = 0.1;       % Filter time constant
phi = 0; theta = 0;  % Initial angles

% Simulate processing 1000 samples
for k = 1:1000
    % z = [gx, gy, gz, ax, ay, az]' from sensors
    z = [gyro_data(:,k); accel_data(:,k)];
    [phi, theta] = Attitude_cf(dt, z, phi, theta, tao);
    roll_deg(k) = phi * 180/pi;
    pitch_deg(k) = theta * 180/pi;
end
```

--------------------------------

### Accelerometer Calibration Function

Source: https://context7.com/rflysim/rflyexpcode/llms.txt

Implements an accelerometer error model for sensor calibration, applying scale factors and bias corrections. Requires raw accelerometer data and an initial parameter guess.

```matlab
function Y = calFunc(x, p)
% Accelerometer error model: Y(i) = norm(Ka * (x(:,i) + ba))
% Inputs:
%   x: Raw accelerometer data, dimension 3 x m (m samples)
%   p: Model parameters [kx, ky, kz, bx, by, bz]' (6 x 1)
%       kx, ky, kz: Scale factors for each axis
%       bx, by, bz: Bias offsets for each axis
% Output:
%   Y: Calibrated gravitational acceleration magnitude (1 x m)

    kx = p(1); ky = p(2); kz = p(3);
    bx = p(4); by = p(5); bz = p(6);

    Ka = [kx 0 0; 0 ky 0; 0 0 kz];  % Scale factor matrix
    ba = [bx by bz]';               % Bias vector

    m = length(x);
    Y = zeros(1, m);
    for i = 1:m
        Y(i) = norm(Ka * (x(1:3, i) + ba));
    end
end
```

```matlab
% Example: Calibrate accelerometer using measured data
% AccRaw contains 3xN matrix of raw accelerometer readings
load('AccRaw.mat');               % Load raw accelerometer data
g = 9.8;                          % Expected gravity (m/s^2)
m = size(AccRaw, 2);              % Number of samples
y_expected = g * ones(m, 1);      % Expected output (gravity magnitude)
p_init = [1.0 1.0 1.0 0.1 0.1 0.1]';  % Initial parameter guess

% Run calibration (requires lm.m optimizer)
p_calibrated = lm('calFunc', p_init, AccRaw, y_expected);
Ka = diag(p_calibrated(1:3));     % Calibrated scale matrix
ba = p_calibrated(4:6);           % Calibrated bias vector
```

=== COMPLETE CONTENT === This response contains all available snippets from this library. No additional content exists. Do not make further requests.