# UAV Attitude Angle Measurement Method Based on Magnetometer-Satellite Positioning System
## 4 Interference Compensation Algorithm  
### Principle of Interference Compensation  
Considering interference conditions such as external magnetic field interference, carrier residual magnetic field, and magnetizing magnetic field interference, the measurement model of the magnetometer can be expressed as:  
\[B_{m}=CB_{e}^{r}+B_{p}+K_{i}CB_{e}^{r}=(I_{3\times 3}+K_{i})CB_{e}^{r}+B_{p} \tag{16}\]  
where \(B_{m}\) is the value measured by the magnetometer; and \(C\) is the conversion matrix from the reference coordinate system to the carrier coordinate system. The reference coordinate system is fixed and can be flexibly defined according to the actual trajectory; \(B_{e}^{r}\) is the local geomagnetic vector field, which can generally be regarded as a fixed vector field; \(B_{p}\) is the external magnetic field interference and carrier remnant magnetic field; and \(K_{i}\) is the magnetic susceptibility matrix.  
The formal transformation of Equation (8) can be obtained as:  
\[B_{m}-(I_{3\times 3}+K_{i})CB_{e}^{r}-B_{p}=0 \tag{17}\]  
Let, \(K=(I_{3\times 3}+K_{i})^{-1}\) and \(B_{pk}=KB_{P}\). Equation (9) can be expressed as:  
\[KB_{m}-CB_{e}^{r}-B_{pk}=0 \tag{18}\]  
From Equation (10), the following linear homogeneous equations is obtained:  
\[AX=0_{N\times 1} \tag{19}\]  
where:  
\[A=\begin{bmatrix}-C_{1}&B_{m,1}^{T}\otimes I_{3\times 3}&-I_{3\times 3}\\ -C_{2}&B_{m,2}^{T}\otimes I_{3\times 3}&-I_{3\times 3}\\ \vdots&\vdots&\vdots\\ -C_{N}&B_{m,N}^{T}\otimes I_{3\times 3}&-I_{3\times 3}\end{bmatrix},X= \begin{bmatrix}B_{e}^{r}\\ vec(K^{T})\\ B_{pk}\end{bmatrix}\]  
\(\otimes\) is the Kronecker product, and \(vec\) represents a matrix algorithm, i.e., a new matrix is formed by stacking the columns of the matrix in order from top to bottom; \(N\) is the data obtained by \(N\) groups of measurements.  
Generally, the total strength of the geomagnetic field is constant and known throughout the entire flight range. Thus, the following equation is obtained:  
\[X^{T}DX=|B_{e}^{r}|^{2} \tag{20}\]  
where:  
\[D=\begin{bmatrix}I_{3\times 3}&0_{3\times 12}\\ 0_{12\times 3}&0_{12\times 12}\end{bmatrix}\]  
The solution of the linear homogeneous equations determined by Equation (11) can be transformed into an optimization problem to obtain the minimum \(X\) value of the objective function \(f(X)=X^{T}A^{T}AX\) under the constraints of Equation (12).  
The Lagrangian multiplier method is used to solve this optimization problem. The Lagrangian function is defined as:  
\[L(X,\lambda)=X^{T}A^{T}AX+\lambda(X^{T}DX-|B_{e}^{r}|^{2}) \tag{21}\]  
Additionally:  
\[\partial L/\partial X=0 \tag{22}\]  
\[\partial L/\partial\lambda=0\]\[A^{T}AX=\lambda DX \tag{23}\] \[X^{T}DX=\left|B_{e}^{T}\right|^{2}\]  
As \(\lambda\) is the generalized characteristic root of matrices \(A^{T}A\) and \(D\), and \(X\) is the eigenvector family corresponding to the characteristic root, we obtain:  
\[X^{T}A^{T}AX=\lambda\left|B_{e}^{T}\right|^{2} \tag{24}\]  
Therefore, when \(\lambda\) assumes the minimum value \(\lambda_{min}\) in the generalized characteristic root, the value of \(f(x)\) is the smallest. The feature vector family corresponding to \(X\) and \(\lambda_{min}\) can be expressed as:  
\[X=\omega v \tag{25}\]  
where \(A\) is the coefficient to be determined, and \(B\) is the eigenvector corresponding to the generalized eigenvalue. Incorporating Equation (25) into Equation (20), we obtain:  
\[\omega=\pm\sqrt{|B_{e}^{r}|^{2}/(v^{T}Dv)} \tag{26}\]