# Roll Angle Estimation Algorithm of Geomagnetic/Gyro Combination Based on an Interacting Multiple-Model Kalman Filter

Authors: Jian-li Su; Hua Wang
Journal: Journal of Sensors
 (2021)

---

## Abstract

The knowledge of the geomagnetic and gyro information that can be used for projectile roll angle is decisive to apply trajectory correction and control law. In order to improve the measurement accuracy of projectile roll angle, an interacting multiple-model Kalman filter (IMMKF) algorithm using gyro angular rate information to geomagnetic sensor information is proposed. Firstly, the data acquisition module of the geomagnetic sensor and the gyroscope sensor is designed, and the test data of the sensors are obtained through the semiphysical experiments. Furthermore, according to the measurement accuracy of each sensor, the algorithm performs the IMMKF process on the geomagnetic/gyro information to get the roll angle. It can be proven by experiments and calculation results that the error of the roll angle obtained after processing by the IMMKF algorithm is close to 2°, which is better than the 5° calculated by adopting the Kalman filter directly with geomagnetic information.


---

## Body

## 1. Introduction

Due to the need of informatization, the guidance of conventional ammunition is an important direction of weapon development. Conventional ammunition with a trajectory correction module will have precision strike capability which can be called guided ammunition. For reducing the production cost of guided ammunition and increasing the combat effectiveness, it is necessary to study the trajectory correction module with low cost and high precision. For most trajectory correction modules, the measurement accuracy of the projectile roll angle is particularly important. Therefore, various measurement methods are proposed by scholars to obtain the roll angle information, which mainly includes the single geomagnetic information, the information fusion of geomagnetic and gyro, the information fusion of geomagnetic and GPS, the information fusion of geomagnetic and infrared, etc. At the same time, to improve the measurement accuracy, scholars have also adopted many estimation algorithms, for example, Kalman filter, adaptive Kalman filter, and a series of improved Kalman filter.Because the geomagnetic sensor has the characteristics of low cost, strong antioverload ability, and no accumulated error, many guided projectiles use a geomagnetic sensor to measure the attitude angle of the projectile during flight [1, 2]. Chen et al. [3] and Tian et al. [4] established a theoretical model to measure the roll angle of the projectile body solely with geomagnetic information and carried out a preliminary simulation experiment, in which the Kalman filter was adopted for data processing. Gao et al. conducted a semiphysical simulation experiment under relatively complicated conditions and used the Kalman filter algorithm to estimate the geomagnetic signal, which obtained the roll angle with relatively good accuracy [5]. Yin et al. denoised the geomagnetic information, measured through the shooting range experiment, based on the wavelet algorithm, and obtained a relatively clean geomagnetic signal [6]. Lu et al. introduced a roll angle compensation method based on geomagnetic detection with a two-axis magnetic sensor [7, 8]. Changey et al. installed a geomagnetic sensor in the projectile and processed the measured data through laboratory experiments and flight tests using the extended Kalman filter to calculate the roll angle [9, 10]. In [11], the measured geomagnetic data is processed in real time through a flight test to calculate the roll angle information of a 40 mm projectile in flight. According to the rotation characteristics of a dual-spin stabilized projectile, Wang et al. introduced a method with an extended Kalman filter to solve the roll estimation by using a spin compensation algorithm [12, 13]. Yang et al. proposed a measurement algorithm based on a geomagnetic sensor to realize real-time accurate measurement of the roll angle of a high-spin rocket [14]. Xiang et al. only used geomagnetic sensors to propose three methods, which proved the feasibility of the method through hardware-in-the-loop simulation experiments, for measuring the attitude of the projectile [15]. However, it is difficult to meet the requirements of accuracy for measuring the roll angle information only through the geomagnetic sensor, because the geomagnetic signal is easily disturbed during the flight of the projectile, resulting in a decrease in measurement accuracy.In view of the problems of using only geomagnetic sensor to measure the roll angle of the projectile, many scholars used different types of sensors for data fusion to measure the roll angle. Qian et al. combined the gyroscope and geomagnetic information by using the extended Kalman filter algorithm to estimate the roll angle information when the GPS was invalid [16]. Aiming at the change of yaw angle that brings about large error by using geomagnetic information in the calculation of roll angle, Zhang et al. proposed a fusion algorithm to solve the roll angle of a missile [17, 18]. The axial angular-velocity information of a gyro was used to assist the three-axis magnetic sensor information. The Kalman filter was used to fuse the data of the dual-axis accelerometer and the gyroscope, which solved the problem of low accuracy in measuring the roll angle of the high-speed rotating projectile [19]. Wang et al. proposed the Sage-Husa adaptive Kalman filter to increase the measurement accuracy of the projectile roll angle, which was indeed effective compared with the extended Kalman filter [20]. They proposed a method to measure the roll angle by using a combination of geomagnetism and infrared and verify the feasibility of this method through semiphysical simulation experiments [21]. In [22], according to the generation mechanism of infrared radiation of the earth, an infrared radiation model of the earth was established, and a method for estimating the attitude of a rotating projectile was proposed. Although the results of the measurement methods and information fusion algorithms mentioned above are satisfactory, they also have their own shortcomings. For example, the infrared sensor is complicated to install on the projectile and must be installed on the surface of the projectile, which is easily affected by the weather and the surface environment. Many scholars also have proposed different algorithms for data fusion and applied them to different fields; in particular, Song et al. used an adaptive extended Kalman filter and adaptive decentralized information filter to fuse the sensor data in the car to solve the difficulty of outdoor positioning when GPS is not available and improve the accuracy [23]. For the problems of high-speed train traffic signal and control, B. Huang and Y. Huang adopted multimode intelligent control of a multidata fusion filter [24]. In order to solve a series of complex problems of nonlinear systems, Jiao et al. proposed a clustering similarity particle filter based on state trajectory consistency and verified the filter [25].Therefore, an adaptive roll estimation algorithm must be designed to solve the problems caused by interference factors and measurement noise. This article introduces an interacting multiple-model Kalman filtering (IMMKF) algorithm, using gyro angular rate information to geomagnetic sensor information, to calculate the roll angle. The research starts from several research foundations, including the establishment of coordinates, theoretical methods of roll estimation, and IMMKF solutions. Secondly, a semiphysical simulation platform is established, and a sensor information acquisition module is designed to measure the roll angle. Finally, experiments show that the IMMKF algorithm can effectively improve the accuracy of the roll angle obtained by using the information of geomagnetism and gyro fusion.


## 2. Roll Angle Estimation Using Geomagnetism and Gyro

Using a combination of geomagnetic sensors and gyroscopes to measure the roll angle information not only solves the problem of insufficient measurement accuracy of a single sensor but also improves the accuracy and hit rate of guided ammunition. At the same time, this combination method can integrate the characteristics of the gyroscope with high measurement accuracy in a short time and the magnetic sensor without drift and with the advantages of small size and low cost, which has a wide range of application prospects.


### 2.1. Principle of Measuring Roll Angle with Geomagnetic Information

The geomagnetic field spreads all over the earth, the distribution law is obvious, and the magnetic field distribution is generally stable. The projectile is always in the earth’s magnetic field during flight, and the real-time roll angle can be estimated accurately. The geomagnetic field element is used to express the magnitude and direction. The strength is a vector, usually represented byT. In order to express the magnitude and direction of the geomagnetic field at a certain point, a coordinate system is generally established with the observation point as the origin to describe the geomagnetic field at this point.The coordinate system is established as shown in Figure1. The origin O represents the center of the earth, the X-axis is parallel to the geographic meridian, and the north is positive. The Y-axis is parallel to the line of latitude and is positive towards the east. The Z-axis perpendicular to the ground plane is positive downwards. In this coordinate system, the angle D between the projection of the geomagnetic field intensity vector T on the horizontal plane and the X-axis is called the magnetic declination. It is generally stipulated that the geomagnetic declination is positive to the east and negative to the west. The angle I at which the vector T deviates from the horizontal plane is called the geomagnetic inclination, which is positive downward and negative upward. The projections of the magnetic field intensity T on the x-, y-, and z-axes are X, Y, and Z, respectively. And H is the projection of T on the horizontal plane. Among them, T, H, I, D, X, Y, and Z are collectively referred to as geomagnetic elements [5].Figure 1
Geomagnetic schematic elements.The geomagnetic element changes with time, usually made up by three elements including magnetic inclination, magnetic declination, and horizontal component, which is used to determine the magnetic field at a certain point. The specific formula can be expressed as follows:(1)HNHEHD=HcosIcosDHcosIsinDHsinI,where the magnetic field intensity is H, I is the magnetic inclination, D is the magnetic declination, and the projected components of the geomagnetic field in the geographic coordinate system are HN, HE, and HD.The projectile coordinate system is fixed on the projectile. In order to determine the attitude angle of the projectile during flight, the projectile coordinate system needs to be established. Its origin is the center of gravity of the projectile, which is denoted asOxyz, as shown in Figure 2.Figure 2
Coordinate transformation.The most intuitive way to describe the attitude is the Euler angle description method. The specific expression is as follows:(2)HNHEHD=cosφcosθ−sinφcosγ+cosφsinθsinγsinφsinγ+cosφsinθcosγsinφcosθcosφcosγ+sinφsinθsinγ−cosφsinγ+sinφsinθcosγ−sinθcosθsinγcosθcosγHxHyHz,where φ is the pitch angle, θ is the yaw angle, and γ is the roll angle. During the flight of the projectile, the yaw angle can be approximately regarded as a constant as it changes very little. Thus, θ=0 is substituted into equation (2). It becomes(3)HNHEHD=cosφ−sinφcosγsinφsinγsinφcosφcosγ−cosφsinγ0sinγcosγHxHyHz.According to the above description, the roll angle calculated by the solution of geomagnetic information can be expressed as(4)γ=arctanHzHy.


### 2.2. Roll Angle Interval Selection

As an inertial device, the gyroscope can be realized autonomously in any environment due to its navigation ability and has the characteristics of relatively high short-term measurement accuracy and small installation volume. It has been widely used in the aerospace field. The gyroscope used in this paper is an angular rate sensor, and the output is angular rate information. It can be integrated within a short sampling interval to obtain high-precision roll angle information, which can be expressed as(5)△γ=∫ti−1tiωtdt,where ti and ti‐1 are two adjacent sampling time points. ωt indicates the angular rate information output by the gyro sensor, and △γ is the angle value obtained by integration.The geomagnetic signal is easily interfered. If the geomagnetic sensor is used to measure the roll angle information, it will affect the accuracy of the roll angle measurement, but the error will not accumulate over time. If the gyroscope is used to measure the roll angle in a short time, the measurement accuracy is high, but there is a cumulative error with time lengthened. Through the fusion of the information measured with the geomagnetic sensor and the gyro sensor, the advantages and disadvantages are complemented, which can obtain a high-precision roll angle.


## 2.1. Principle of Measuring Roll Angle with Geomagnetic Information

The geomagnetic field spreads all over the earth, the distribution law is obvious, and the magnetic field distribution is generally stable. The projectile is always in the earth’s magnetic field during flight, and the real-time roll angle can be estimated accurately. The geomagnetic field element is used to express the magnitude and direction. The strength is a vector, usually represented byT. In order to express the magnitude and direction of the geomagnetic field at a certain point, a coordinate system is generally established with the observation point as the origin to describe the geomagnetic field at this point.The coordinate system is established as shown in Figure1. The origin O represents the center of the earth, the X-axis is parallel to the geographic meridian, and the north is positive. The Y-axis is parallel to the line of latitude and is positive towards the east. The Z-axis perpendicular to the ground plane is positive downwards. In this coordinate system, the angle D between the projection of the geomagnetic field intensity vector T on the horizontal plane and the X-axis is called the magnetic declination. It is generally stipulated that the geomagnetic declination is positive to the east and negative to the west. The angle I at which the vector T deviates from the horizontal plane is called the geomagnetic inclination, which is positive downward and negative upward. The projections of the magnetic field intensity T on the x-, y-, and z-axes are X, Y, and Z, respectively. And H is the projection of T on the horizontal plane. Among them, T, H, I, D, X, Y, and Z are collectively referred to as geomagnetic elements [5].Figure 1
Geomagnetic schematic elements.The geomagnetic element changes with time, usually made up by three elements including magnetic inclination, magnetic declination, and horizontal component, which is used to determine the magnetic field at a certain point. The specific formula can be expressed as follows:(1)HNHEHD=HcosIcosDHcosIsinDHsinI,where the magnetic field intensity is H, I is the magnetic inclination, D is the magnetic declination, and the projected components of the geomagnetic field in the geographic coordinate system are HN, HE, and HD.The projectile coordinate system is fixed on the projectile. In order to determine the attitude angle of the projectile during flight, the projectile coordinate system needs to be established. Its origin is the center of gravity of the projectile, which is denoted asOxyz, as shown in Figure 2.Figure 2
Coordinate transformation.The most intuitive way to describe the attitude is the Euler angle description method. The specific expression is as follows:(2)HNHEHD=cosφcosθ−sinφcosγ+cosφsinθsinγsinφsinγ+cosφsinθcosγsinφcosθcosφcosγ+sinφsinθsinγ−cosφsinγ+sinφsinθcosγ−sinθcosθsinγcosθcosγHxHyHz,where φ is the pitch angle, θ is the yaw angle, and γ is the roll angle. During the flight of the projectile, the yaw angle can be approximately regarded as a constant as it changes very little. Thus, θ=0 is substituted into equation (2). It becomes(3)HNHEHD=cosφ−sinφcosγsinφsinγsinφcosφcosγ−cosφsinγ0sinγcosγHxHyHz.According to the above description, the roll angle calculated by the solution of geomagnetic information can be expressed as(4)γ=arctanHzHy.


## 2.2. Roll Angle Interval Selection

As an inertial device, the gyroscope can be realized autonomously in any environment due to its navigation ability and has the characteristics of relatively high short-term measurement accuracy and small installation volume. It has been widely used in the aerospace field. The gyroscope used in this paper is an angular rate sensor, and the output is angular rate information. It can be integrated within a short sampling interval to obtain high-precision roll angle information, which can be expressed as(5)△γ=∫ti−1tiωtdt,where ti and ti‐1 are two adjacent sampling time points. ωt indicates the angular rate information output by the gyro sensor, and △γ is the angle value obtained by integration.The geomagnetic signal is easily interfered. If the geomagnetic sensor is used to measure the roll angle information, it will affect the accuracy of the roll angle measurement, but the error will not accumulate over time. If the gyroscope is used to measure the roll angle in a short time, the measurement accuracy is high, but there is a cumulative error with time lengthened. Through the fusion of the information measured with the geomagnetic sensor and the gyro sensor, the advantages and disadvantages are complemented, which can obtain a high-precision roll angle.


## 3. IMMKF Estimation Algorithm

After the Kalman filter has been successfully applied in the design of integrated navigation systems, it has gradually become a major technical means of multisensor data fusion systems and has been widely used in various fields. Kalman filtering can be used to estimate states or parameters. It is a highly reliable signal processing method that can process the measurement equations and state equations of the system at the same time.


### 3.1. Standard Kalman Filter

The standard Kalman filter uses the statistical characteristics of noise and takes system observations as input and the estimated value of the parameter or system state as output to estimate the required signal according to the time updating and observation updating algorithm. Assume that the discrete linear system equation and the measurement equation are set, respectively [26–28]:(6)Xk=Φk,k−1Xk−1+ωk−1,(7)Zk=HkXk+vk,where Xk is the state vector, Φk,k−1 is the state one-step transition matrix, ωk−1 is the system noise vector, and Zk is the measurement vector. Hk is the measurement matrix, and vk is a measurement noise vector. ωk−1 and vk are uncorrelated zero-mean Gaussian white noises, and the variances are Qk and Rk, respectively. The estimate X^k of Xk can be obtained by the following process:The one-step prediction of the state is(8)X^k,k−1=Φk,k−1X^k−1.The state one-step prediction mean square error is(9)Pk,k−1=Φk,k−1Pk−1Φk,k−1T+QK−1.The filter gain is(10)Kk=Pk,k−1HkTHkPk,k−1HkT+Rk−1,where Pk,k−1 is the prediction state covariance matrix.The state is estimated as(11)X^k=X^k,k−1+KkZk−HkX^k,k−1,where Kk is the filter gain matrix.The mean square error of the state estimation is(12)Pk=I−KkHkPk,k−1,where Pk is the variance matrix and I is the identity matrix.


### 3.2. IMMKF Estimation Algorithm

In the actual measurement system, the measurement matrix contains multiple sensors and the measurement data of each sensor axis, and the measurement model parameters of each sensor axis may be different. Therefore, this work proposes an interactive multimodel algorithm, which greatly reduces the number of filter models by separately estimating the data measured with sensors of each axis. In this paper, the geomagnetic and gyro sensor combination module is designed. The raw data measured with the sensors are obtained through the data acquisition and storage unit, and then, the roll angle information is obtained by using the IMMKF algorithm. The schematic of the theoretic method is given in Figure3.Figure 3
Schematic of theoretic roll estimation method.An interacting multiple model assumes that the transfer between models obeys the Markov chain, interacts with the initial input of each model, and reinitializes the state estimation value and covariance matrix of each Kalman filter module. The initial value of each filter is based on the previous one. Periodic state estimation and conditional transition probability calculation and the filtered output interaction value were obtained by correction, avoiding the phenomenon of jumping and reconvergence when the parameters of a single model are switched [29]. The structure diagram is shown in Figure 4.Figure 4
The structure of the IMMKF algorithm.The detailed steps of IMMKF are as follows.(1)
Input interactionInteract with the output results of each model filter in the last cycle as the initial state in the current cycle. The interaction process is as follows:(13)X^k−1∣k−1,jin=∑i=1NX^k−1∣k−1,iuk−1∣k−1,i∣j,where the associated probability for the model i is denoted by uk−1∣k−1,i∣j.(14)Pk−1∣k−1,jin=∑i=1Nuk−1∣k−1,i∣jP~k−1∣k−1,i+X^k−1∣k−1,i−Xk−1∣k−1,jinX∧k−1∣k−1,i−Xk−1∣k−1,jinT,where(15)uk−1∣k−1,i∣j=Pijuk−1,ic¯j,c¯j=∑i=1NPijuk−1,i,where X^k−1∣k−1,i represents the state output value of filter i at time k−1, P~k−1∣k−1,i is the corresponding covariance matrix estimate, X^k−1∣k−1,jin is the state interaction value of model j at time k−1, Pk−1∣k−1,jin represents the interaction value of the covariance matrix of model j at time k−1, uk−1∣k−1,i∣j is the mixed transition probability of model i transferring to model j at this moment, and c¯j denotes the normalization constant value.(2)
Filter processingKalman filtering is performed on the interaction results of each model obtained by the above formula, and the updated value of each filter state at timek is obtained.First calculate the prediction state and prediction covariance matrix of each model atk time. The specific process is as follows:(16)x^k∣k−1,j=Fk−1,jX^k−1∣k−1,jin,where Fk−1,j denotes the state transition matrix.(17)Pk∣k−1,j=Fk−1,jPk−1∣k−1,jinFk−1,jT+Qk−1,j.Then, the state estimation and covariance matrix estimation of each model at timek are revised:(18)Kk,j=Pk∣k−1,jHk,jTHk,jPk∣k−1,jHk,jT+Rk,j−1=Pk∣k−1,jHk,jTSk,j−1,x^k∣k,j=x^k∣k−1,j+Kk,jz~k,jHk,jx^k∣k−1,j=x^k∣k−1,j+Kk,jρk,j,Pk∣k,j=I−Kk,jHk,jPk∣k−1,j,where(19)ρk,j=z~k,j−Hk,jx^k∣k−1,j,Sk,j=Hk,jPk∣k−1,jHk,jT+Rk,j=Pzz,k∣k−1,j+Rk,j,z^k∣k−1,j=Hk,jx^k∣k−1,j.(3)
Probability updatesUpdate the probability of each model at timek:(20)uk,j=Λk,jc¯j∑j=1NΛk,jc¯j,where(21)Λk,j=exp−1/2ρk,jTSk,j−1ρk,j2πSk,j1/2.(4)
Output interactionAccording to the probability of each model at timek, their filtering results are weighted and fused, and the final state estimate and covariance matrix estimate of the target at time k are output:(22)X^k∣k=∑j=1Nuk,jx^k∣k,j,P~k∣k=∑j=1Nuk∣k,jPk∣k+x^k∣k,j−X^k∣kx∧k∣k,j−X∧k∣kT.


## 3.1. Standard Kalman Filter

The standard Kalman filter uses the statistical characteristics of noise and takes system observations as input and the estimated value of the parameter or system state as output to estimate the required signal according to the time updating and observation updating algorithm. Assume that the discrete linear system equation and the measurement equation are set, respectively [26–28]:(6)Xk=Φk,k−1Xk−1+ωk−1,(7)Zk=HkXk+vk,where Xk is the state vector, Φk,k−1 is the state one-step transition matrix, ωk−1 is the system noise vector, and Zk is the measurement vector. Hk is the measurement matrix, and vk is a measurement noise vector. ωk−1 and vk are uncorrelated zero-mean Gaussian white noises, and the variances are Qk and Rk, respectively. The estimate X^k of Xk can be obtained by the following process:The one-step prediction of the state is(8)X^k,k−1=Φk,k−1X^k−1.The state one-step prediction mean square error is(9)Pk,k−1=Φk,k−1Pk−1Φk,k−1T+QK−1.The filter gain is(10)Kk=Pk,k−1HkTHkPk,k−1HkT+Rk−1,where Pk,k−1 is the prediction state covariance matrix.The state is estimated as(11)X^k=X^k,k−1+KkZk−HkX^k,k−1,where Kk is the filter gain matrix.The mean square error of the state estimation is(12)Pk=I−KkHkPk,k−1,where Pk is the variance matrix and I is the identity matrix.


## 3.2. IMMKF Estimation Algorithm

In the actual measurement system, the measurement matrix contains multiple sensors and the measurement data of each sensor axis, and the measurement model parameters of each sensor axis may be different. Therefore, this work proposes an interactive multimodel algorithm, which greatly reduces the number of filter models by separately estimating the data measured with sensors of each axis. In this paper, the geomagnetic and gyro sensor combination module is designed. The raw data measured with the sensors are obtained through the data acquisition and storage unit, and then, the roll angle information is obtained by using the IMMKF algorithm. The schematic of the theoretic method is given in Figure3.Figure 3
Schematic of theoretic roll estimation method.An interacting multiple model assumes that the transfer between models obeys the Markov chain, interacts with the initial input of each model, and reinitializes the state estimation value and covariance matrix of each Kalman filter module. The initial value of each filter is based on the previous one. Periodic state estimation and conditional transition probability calculation and the filtered output interaction value were obtained by correction, avoiding the phenomenon of jumping and reconvergence when the parameters of a single model are switched [29]. The structure diagram is shown in Figure 4.Figure 4
The structure of the IMMKF algorithm.The detailed steps of IMMKF are as follows.(1)
Input interactionInteract with the output results of each model filter in the last cycle as the initial state in the current cycle. The interaction process is as follows:(13)X^k−1∣k−1,jin=∑i=1NX^k−1∣k−1,iuk−1∣k−1,i∣j,where the associated probability for the model i is denoted by uk−1∣k−1,i∣j.(14)Pk−1∣k−1,jin=∑i=1Nuk−1∣k−1,i∣jP~k−1∣k−1,i+X^k−1∣k−1,i−Xk−1∣k−1,jinX∧k−1∣k−1,i−Xk−1∣k−1,jinT,where(15)uk−1∣k−1,i∣j=Pijuk−1,ic¯j,c¯j=∑i=1NPijuk−1,i,where X^k−1∣k−1,i represents the state output value of filter i at time k−1, P~k−1∣k−1,i is the corresponding covariance matrix estimate, X^k−1∣k−1,jin is the state interaction value of model j at time k−1, Pk−1∣k−1,jin represents the interaction value of the covariance matrix of model j at time k−1, uk−1∣k−1,i∣j is the mixed transition probability of model i transferring to model j at this moment, and c¯j denotes the normalization constant value.(2)
Filter processingKalman filtering is performed on the interaction results of each model obtained by the above formula, and the updated value of each filter state at timek is obtained.First calculate the prediction state and prediction covariance matrix of each model atk time. The specific process is as follows:(16)x^k∣k−1,j=Fk−1,jX^k−1∣k−1,jin,where Fk−1,j denotes the state transition matrix.(17)Pk∣k−1,j=Fk−1,jPk−1∣k−1,jinFk−1,jT+Qk−1,j.Then, the state estimation and covariance matrix estimation of each model at timek are revised:(18)Kk,j=Pk∣k−1,jHk,jTHk,jPk∣k−1,jHk,jT+Rk,j−1=Pk∣k−1,jHk,jTSk,j−1,x^k∣k,j=x^k∣k−1,j+Kk,jz~k,jHk,jx^k∣k−1,j=x^k∣k−1,j+Kk,jρk,j,Pk∣k,j=I−Kk,jHk,jPk∣k−1,j,where(19)ρk,j=z~k,j−Hk,jx^k∣k−1,j,Sk,j=Hk,jPk∣k−1,jHk,jT+Rk,j=Pzz,k∣k−1,j+Rk,j,z^k∣k−1,j=Hk,jx^k∣k−1,j.(3)
Probability updatesUpdate the probability of each model at timek:(20)uk,j=Λk,jc¯j∑j=1NΛk,jc¯j,where(21)Λk,j=exp−1/2ρk,jTSk,j−1ρk,j2πSk,j1/2.(4)
Output interactionAccording to the probability of each model at timek, their filtering results are weighted and fused, and the final state estimate and covariance matrix estimate of the target at time k are output:(22)X^k∣k=∑j=1Nuk,jx^k∣k,j,P~k∣k=∑j=1Nuk∣k,jPk∣k+x^k∣k,j−X^k∣kx∧k∣k,j−X∧k∣kT.


## 4. Results and Discussion

In order to verify that the IMMKF algorithm can effectively improve the accuracy of the roll angle obtained by using the information of geomagnetism and gyro fusion, an experimental simulation platform is established. First, a two-axis geomagnetic sensor and a single-axis gyro module are designed. Then, as the motor and the materials of platform are very susceptible to interference with the geomagnetic sensor, the special treatment is carried out on the structure of the platform so that the installation position of the geomagnetic sensor is as far away as possible from the motor, and the material of the platform is almost aluminum. The structure of the semiphysical platform and the installation position of the sensors are shown in Figure5.Figure 5
Arrangement of the instrumentation: 1—stepping motor, 2—extended aluminum rod, 3—aluminum transmission mechanism, 4—battery, 5—computing unit, 6—sensors, 7—two-axis geomagnetic sensor, and 8—single-axis gyro sensor.For better simulating the rotation state of the missile body during flight, the sensor data acquisition module is set 10 r/s, and the sampling frequency is 400 Hz. To verify the stability of the test platform and sensor module during the experiment, a relatively large disturbance is artificially added. The original data output waveform of the two-axis geomagnetic sensor measured through the test is shown in Figure6.Figure 6
The original geomagnetic output waveform.As shown in Figure3, the Kalman filter is applied to the data of two axes measured with the geomagnetic sensor, respectively. The geomagnetic signal on each axis is a one-dimensional vector, according to formulas (6) and (7) (Φ=1, H=1). According to the result measured with the experimental simulation platform, Q=0.001 and R=0.01 can be set. Through Kalman filter processing, the geomagnetic signal has been significantly improved, which has become smoother and closer to the sinusoidal characteristics. And it is more conducive to the calculation of the roll angle, as shown in Figure 7.Figure 7
The processed data by using the Kalman filter.The original signal of the geomagnetic sensor and the signal after Kalman filter processing are used to calculate the roll angle information according to formula (4), and the results are shown in Figure 8. Figure 9 is the roll angle error obtained by comparison with the theoretical value. Through the above, we can see that the roll angle error obtained through Kalman filter processing is about 5°, while the roll angle error calculated directly from the measured raw data is about 8°, indicating that the Kalman filter does improve the measurement accuracy of the roll angle.Figure 8
Roll angle of the magnetic sensor.Figure 9
Roll angle error of the magnetic sensor.As shown in Figure3, the Kalman filter is applied to the data measured with the gyroscope sensor. The signal measured with the gyroscope sensor is a one-dimensional vector, according to formulas (6) and (7) (Φ=1, H=1). According to the result measured with the experimental simulation platform, Q=10 and R=1000 can be set. Under the same experimental conditions, the gyroscope signal measured with the semiphysical simulation platform and the signal after Kalman filter processing are shown in Figure 10. Compared with the angular rate calculated by using geomagnetic information, as shown in Figure 11, it can be seen that the angular rate accuracy of the gyro is very high, and it further illustrates that the accuracy of the roll angle obtained by the gyro sensor in a short time is higher than that of the geomagnetic sensor.Figure 10
Roll rotation rate of the gyro sensor.Figure 11
Roll rotation rate.In order to complement the advantages of the geomagnetic sensor and the gyro sensor, the IMMKF algorithm is adopted to fuse the data of each sensor to obtain the roll angle information. The core idea of this algorithm is to first use the Kalman filter to process the two-axis signals of the geomagnetic sensor separately and obtain the roll angle information according to formula (4). Then, use the Kalman filter to process the signal of the gyroscope sensor and calculate the angle that the projectile has turned in a short time according to formula (5). Finally, the angle information obtained by the two sensors is fused to obtain the higher-precision roll angle information. The estimation result is shown in Figure 12. It can be seen from Figure 13 that the error of the roll angle obtained after processing by the IMMKF algorithm is close to 2°, which is better than the 5° calculated by adopting the Kalman filter directly with geomagnetic information.Figure 12
Roll angle.Figure 13
Roll angle error.


## 5. Conclusions

In this work, as the geomagnetic signal is very susceptible to interference, especially the shield of the projectile body to magnetic field environment, we can see a serious decrease in the measurement accuracy. Meanwhile, the angle information of the output signal of the gyro is obtained through integration, but there will be unrecoverable cumulative error if the integration is carried out for a long time. A novel measurement method is proposed to estimate the roll angle through the fusion of geomagnetic and gyro information which is based on the IMMKF algorithm. The main contributions are as follows:(1)
A semiphysical simulation platform is built for the rotation characteristics of the projectile during flight. In order to obtain the roll angle information, a geomagnetic sensor module and gyroscope sensor module are designed, respectively(2)
The IMMKF algorithm is used to fuse the geomagnetic and gyro data measured with the semiphysical simulation platform to obtain the roll angle information(3)
It is verified by comparison that the accuracy of the roll angle obtained by fusing geomagnetism and gyro information is better than that obtained by only using geomagnetic information. It also shows that the IMMKF algorithm is better than the Kalman filter in improving the accuracy of measuring the roll angleThe future work is to improve the roll angle accuracy when the projectile is under the great disturbance.



---

*Source: 4184391-2021-09-08.xml*