Kalman Filter for Gyroscope and Accelerometer Angle Fusion

The state equation describes how the state vector changes, where the derivative of the state vector is a linear combination of the current state plus a linear combination of external inputs. This focuses on dynamics of each state variable and how they relate to each other. The matrix form uses matrix A (describing internal state connections and underlying system dynamics) and matrix B (describing how inputs enter the system and which states they affect).

State space representation is a method used in control systems to describe system behavior. To predict system behavior after input application, one must know the initial state and the input. State variables completely describe the system's condition at any time. State space equations are: x_dot = Ax + Bu (state equation) and y = Cx + Du (output equation). Matrix operations follow specific rules: multiplication requires columns of first matrix to equal rows of second matrix. Matrix multiplication is not commutative but is associative and distributive. The transpose swaps rows and columns. The cofactor of element a_ij is: C_ij = (-1)^(i+j) × det(M_ij). The inverse is: A^(-1) = (1/det(A)) × adj(A). These concepts form the mathematical foundation for analyzing and designing control systems.

When state space equations are linear, they can be rewritten in matrix form. The general form consists of two matrix equations: the state equation and the output equation. The state equation is: dx/dt = A*x + B*u, where x is the vector of state variables, u is the vector of inputs, A is the system matrix, and B is the input matrix. The output equation is: y = C*x + D*u, where y is the vector of outputs, C is the output matrix, and D is the feedthrough matrix. Each row of the A matrix corresponds to one state equation, and each row of the C matrix corresponds to one output equation.

State space equations consist of two types: the state equation (X_dot = AX + BU) and the output equation (Y = CX + DU). For a system with R inputs, M outputs, and N state variables: A is the N×N state matrix, B is the N×R input matrix, C is the M×N output matrix, and D is the M×R direct transmission matrix. X is the N×1 state vector, Y is the M×1 output vector, and U is the R×1 input vector. These equations completely describe the system dynamics and are derived from block diagrams by adding summing points to obtain the state and output equations.

The state equation ẋ(t) = A·x(t) + B·u(t) provides a compact matrix representation of system dynamics. Matrix A (n×n) contains coefficients determining how states influence each other, while matrix B (n×m) contains coefficients showing how inputs affect states. The state vector x(t) is n×1 and input vector u(t) is m×1. This matrix formulation allows efficient computation and analysis of multi-input multi-output systems using linear algebra techniques.

This lesson introduces essential probability and statistical concepts for analyzing noisy signals in stochastic control systems, covering sample space, histograms, empirical cumulative distribution functions, probability density functions, expected value, variance, covariance matrices, and autocorrelation matrices, with emphasis on uniform and Gaussian distributions and their applications in Kalman filter design for state estimation from partial measurements.

The Kalman filter estimates system state by combining measurements with a model of system behavior. X represents the state, Z represents measurements, and the estimate is the best guess of the true state. Gaussian noise follows a normal distribution where the mean is most likely and large errors drop off as you move away. One standard deviation captures approximately 68% of values. Covariance matrices describe expected errors in each state variable and how those errors relate to each other, with high covariance indicating closely related variables.

Gaussian distributions possess key properties essential for the Kalman filter: (1) Marginalization preserves Gaussian form - if you integrate out variables from a multivariate Gaussian, the resulting distribution remains Gaussian; (2) Conditional distributions remain Gaussian - given one subset of variables, the distribution of another subset remains Gaussian; (3) Convolution of Gaussians yields a Gaussian. These properties ensure that when applying the Bayesian recursive filter framework to Gaussian distributions, all intermediate results remain Gaussian, allowing the entire estimation process to be represented and computed within the Gaussian framework.

Under Gaussian assumptions, the conditional distribution of the state x given observations up to time t is Gaussian with mean x̂(t) and covariance Σ(t). This distribution completely characterizes uncertainty about the state. The Kalman filter provides optimal estimates by computing the conditional mean and covariance. The beauty of Gaussian assumptions is that only two parameters (mean and covariance) are needed to fully describe the distribution, making estimation computationally efficient.

A fundamental hypothesis in weighted least squares state estimation is that measurement errors follow a normal (Gaussian) distribution. This distribution is characterized by two parameters: mean (μ) and variance (σ²). Manufacturers of measurement devices typically provide these statistical parameters, which are assumed to be known. If measurements have non-Gaussian error distributions, the WLS method cannot be directly applied, requiring alternative approaches.

The body-fixed frame is a local coordinate system rigidly attached to the UAV, moving and rotating with the vehicle, centered at the center of mass with X-axis forward, Y-axis sideways, and Z-axis downward. This frame is essential for describing UAV dynamics, orientation, forces, and rotational moments, enabling definition of the six-degree-of-freedom model. Transformations between inertial and body frames require rotation matrices based on Euler angles (roll, pitch, yaw). The transformation equations V_inertial = R × V_body and V_body = R × V_inertial enable conversion of forces, velocities, and accelerations between coordinate systems. Controllers use these transformations to convert position requirements in the inertial frame into appropriate motor commands in the body frame, accounting for the drone's orientation during flight.

The roll-pitch-yaw Euler angles (Z-Y-X convention) describe the orientation of a rigid body through three sequential rotations: first yaw rotation around the Z-axis by angle θ₁, then pitch rotation around the rotated Y-axis by angle θ₂, and finally roll rotation around the twice-rotated X-axis by angle θ₃. The direction cosine matrix M that transforms vector coordinates from the body-fixed frame B to the inertial frame 0 is obtained by multiplying the individual rotation matrices: M = R_z(θ₁) × R_y(θ₂) × R_x(θ₃), where R_z, R_y, and R_x represent rotation matrices around the respective axes. This matrix contains the direction cosines that relate the orientations of the two coordinate systems and satisfies the orthogonality property M^T × M = I, ensuring proper rotation representation.

Euler angles (φ, θ, ψ) are three angles that describe the relative orientation between two coordinate frames through a specific rotation sequence: first a rotation about the z-axis (yaw), then about the resulting y-axis (pitch), and finally about the resulting x-axis (roll/bank). This Euler rotation sequence transforms a vector from the reference frame (Northeast Down) to the body frame using three intermediate frames, and the resulting Direction Cosine Matrix (DCM) is unitary (its inverse equals its transpose), meaning it is never singular. The DCM can also be used to recover the Euler angles, and any orientation can alternatively be represented as a single rotation about a unique axis, which corresponds to the eigenvector associated with the eigenvalue of 1 in the DCM.

State-space methods are extensively applied in aerospace systems including aircraft, spacecraft, and drones. Common state variables include position (x, y, z), velocity components, Euler angles (roll, pitch, yaw), and their time derivatives. Euler angles describe orientation through three successive rotations: roll about z-axis, pitch about new y-axis, and yaw about new z-axis. Rotation matrices constructed from Euler angles transform vectors between body-fixed and inertial frames. This coordinate frame transformation is essential for expressing dynamic equations in physically meaningful terms and designing effective control laws for attitude control.

Quadcopter orientation is described using three Euler angles: roll (φ) about X-axis, pitch (θ) about Y-axis, and yaw (ψ) about Z-axis. As the quadcopter rotates, its body-fixed frame becomes misaligned with the inertial frame. To analyze this orientation change, three intermediate reference frames are used: R1 (yaw rotation about common Z-axis), R2 (pitch rotation about common Y-axis), and R3 (roll rotation about common X-axis). Each rotation is a simple angular motion with angular velocity equal to the rate of change of the respective angle. The addition theorem for angular velocities states that the angular velocity of any frame relative to the inertial frame equals the sum of angular velocities between consecutive intermediate frames. Matrix representations of each simple rotation are combined to express the complete orientation transformation. The absolute angular velocity components p, q, r in the body-fixed frame are expressed in terms of Euler angle rates φ̇, θ̇, ψ̇.