**6.2 Kalman filter**

Although dynamical behavior of a linear system is often expressed in the form of continuous differential equations, in practice, the measurements are provided at discrete intervals of time. The form of difference equations are used as below:

$$\mathbf{x}\_{k} = \mathbf{F}\_{k-1}\mathbf{x}\_{k-1} + \mathbf{B}\_{k-1}\mathbf{u}\_{k-1} + \mathbf{w}\_{k-1} \tag{23}$$

where the *n* vector, **x***k*, is called the state of the system at time *k*, **u***k*-1 is a *p* vector of deterministic inputs and **w***k*-1 is the system noise at time *k*-1. **F***k*-1 is an *n*×n state transition matrix from time *k*-1 to time *k*, known as the system matrix, and **B***k*-1 is an *n*×*p* system input matrix. **F***k*-1 and **B***k*-1 are constant or time varying matrices. The system (or process) noise **w***<sup>k</sup>* is normally distributed with zero mean and a power spectral density of **Q***k*-1.

The vector **x***<sup>k</sup>* contains all the information regarding the present state of the system, but cannot be measured directly. Instead, only the system's *m* vector, **z***k*, is available. This measurement vector is a linear combination of the state, **x***k,* that is corrupted by the measurement noise **v***k*. This can be expressed in terms of the system state by the following equation:

$$\mathbf{z}\_k = \mathbf{H}\_k \mathbf{x}\_k + \mathbf{v}\_k \tag{24}$$

where **H***k* is an *m*×n measurement matrix. The measurement (or observation) noise, **v***k*, has zero-mean and is normally distributed, with power spectral density **R***k*.

The Kalman filter for the system described here seeks to provide the best estimates of the states, **x***k*, using the measurements, **z***k*, model of the system provided by the matrices **F***k*, **B***<sup>k</sup>* and **H***k*, and knowledge of the system and measurement statistics given in the matrices **Q***k* and **R***k*.

From (23) and (24), we can recognize that Kalman filter is a recursive filter: only the state of last time step and the current measurement are used. The estimation is conceptualized as two distinct phases, "Predict" and "Update", described by two distinct set of equations. In the first phase, the prediction is based on previous best estimate. The result is called *a priori* state estimate. In the second phase, the updating is done based on the *a priori* state estimate with new measurement. The result is *a posteriori* state estimate. The two set of distinct equations are as below:

#### **The prediction process**

80 Magnetic Sensors – Principles and Applications

**—**Estimated

In the setup of experiments related to Fig 8, the walk is along an outdoor J-shaped path near

The use of higher quality inertial/magnetic sensor modules will improve positioning accuracy but the cost of such a solution is often much higher. An alternative solution is to use various positioning data from multiple sources such as radio navigation aids and additional sensors onboard. Through the control applied to the drift error, a positioning system using multiple data sources "fused" together is able to outperform a system using a single data source. Complementary filtering techniques were used to integrate data from different positioning aids in the past. Nowadays, Kalman filtering is popular in combination of two estimates of a variable. For example, IPS data can be integrated with GPS data to

Although dynamical behavior of a linear system is often expressed in the form of continuous differential equations, in practice, the measurements are provided at discrete

a playground. The plotted trace recorded is similar to the actual path walked.

provide enforced positioning ability in both indoor and outdoor environment.

intervals of time. The form of difference equations are used as below:

Fig. 8. Tracking the walk on a J-shaped path at a park

**6. Sensor fusion and Kalman filtering** 

**6.1 Sensor fusion** 

**6.2 Kalman filter** 

Predicted (*a priori*) state estimate is the best estimate of the state at time *k*-1, denoted as **x***k*-1/*k*-1. Since the process noise, **w***k*-1, is normally distributed with zero-mean, the best prediction of is given by

$$\mathbf{x}\_{k/k-1} = \mathbf{F}\_{k-1} \mathbf{x}\_{k-1/k-1} \tag{25}$$

while the predicted (*a priori*) estimate covariance at time *k* predicted at time *k-*1, is given by

$$\mathbf{P}\_{k/k-1} = \mathbf{F}\_{k-1} \mathbf{P}\_{k-1/k-1} \mathbf{F}\_{k-1}^T + \mathbf{Q}\_{k-1} \tag{26}$$

#### **The measurement update**

On the arrival of new measurement **z***k*, at time *k*, it is compared with the *a priori* state estimate of the measurement. The measurement is then used to update the prediction to generate a best estimate, i.e., the *a posteriori* state estimate at time *k-*1:

$$\mathbf{x}\_{k/k} = \mathbf{x}\_{k/k-1} - \mathbf{K}\_k [\mathbf{H}\_k \mathbf{x}\_{k/k-1} - \mathbf{z}\_k] \tag{27}$$

The Application of Magnetic Sensors in Self-Contained Local Positioning 83

/ 1 1 1/ 1 1 1

T 1

*k kk k k* / 1

Kalman filter, sometimes with other techniques, have already been used for data fusion and positioning. Recently, Zhao & Wang (2011) use EKF for the fusion of data from inertial sensors, ultrasonic sensors, and magnetic sensor. A 3D magnetic sensor and a 3D accelerometer are combined to measure the gravity and the earth's magnetic field for the static orientation. A 3D gyroscope is used to obtain the dynamic orientation. The integral drift of accelerometer is periodically calibrated by an ultrasonic sensor. All these data are fused by EKF. The measurements are the position obtained by ultrasonic sensor and the orientation obtained by magnetic sensors and accelerometers. The states incorporated the position, velocity, and orientation. The experimental results demonstrated that the

In another research, Shen et al (2011) apply Kalman filter, together with Fast Orthogonal Search (FOS) in the 2D navigation for land vehicles. Kalman filter is used for data fusion of GPS and inertial sensors, i.e. the integration of GPS, IMU and vehicle built-in odometer. FOS is a nonlinear error identification technique. It is used to model non-stationary stochastic sensor errors and non-linear inertial errors in the study. The road test trajectories confirm that a module with both Kalman filtering and FOS technique outperforms that with Kalman

It can be concluded that improvement of positioning accuracy will be possible if data from multiple sensors, including magnetic sensors, are fused by Kalman filter. Further

improvement is achievable if Kalman filtering is combined with other techniques.

Measurement residual is calculated through linearization of system dynamics

**The measurement update** 

The relevant covariance is

The suboptimal Kalman gain is

The state estimate is updated as

**6.4 Application examples** 

filtering only.

The relevant estimate covariance is also updated as

uncertainty of orientation and position is lowered.

**x xu** *kk k k k* / 1 1/ 1 1 *f* , (34)

**yz x** *k k kk h* / 1 (36)

<sup>T</sup> **S HP H R** *k k kk k k* / 1 (37)

**K P HS** (38)

**x x Ky** *kk kk k k* / /1 (39)

**P I KH P** *kk k k kk* / / 1 (40)

*<sup>T</sup>* **P FP F Q** *kk k k k k k* (35)

Accordingly, the updated (*a posteriori*) estimate covariance is:

$$\mathbf{P}\_{k/k} = \mathbf{P}\_{k/k-1} - \mathbf{K}\_k \mathbf{H}\_k \mathbf{P}\_{k/k-1} \tag{28}$$

where the optimal Kalman gain matrix is given by:

$$\mathbf{K}\_k = \mathbf{P}\_{k/k-1} \mathbf{H}\_k^T \left[ \mathbf{H}\_k \mathbf{P}\_{k/k-1} \mathbf{H}\_k^T + \mathbf{R}\_k \right] \tag{29}$$

Typically, the two phases alternate, with the prediction advancing the state until the next scheduled measurement, and the update incorporating the measurement.

### **6.3 Extended Kalman filter**

Kalman filter is optimal in either the least square sense or maximum likelihood sense if it is applied to linear dynamic system with zero mean, normally distributed noise. Kalman filter becomes not optimal when the system is nonlinear, or the noise is not normally distributed.

EKF is the nonlinear version of the Kalman filter. EKF linearizes the dynamic system about the current mean and covariance. The estimate is suboptimal, i.e. it is an approximation of the optimal estimate.

In EKF, the state transition and measurement models may be non-linear differentiable functions:

$$\mathbf{x}\_{k} = f\left(\mathbf{x}\_{k-1}, \mathbf{u}\_{k-1}\right) + \mathbf{w}\_{k-1} \tag{30}$$

$$\mathbf{z}\_k = h(\mathbf{x}\_k) + \mathbf{v}\_k \tag{31}$$

where system noise **w***k* is normally distributed with zero mean and covariance **Q***k* while measurement noise **v***k* is zero mean normally distributed with covariance **R***k*. The functions *f* and *h* can be used to estimate the current state and measurement from previous state and measurement respectively. However, the current covariance of system noise and measurement noise cannot be estimated from these two functions directly. Instead the Jacobians, matrices of partial derivatives, must be calculated. The state transition and measurement matrices are defined as:

$$\mathbf{F}\_{k-1} = \frac{\partial f}{\partial \mathbf{x}}\bigg|\_{\mathbf{x}\_{k-1/k-1}, \mathbf{u}\_{k-1}}\tag{32}$$

$$\mathbf{H}\_k = \frac{\partial h}{\partial \mathbf{x}}\bigg|\_{\mathbf{x}\_{k/k-1}}\tag{33}$$

Similar with Kalman filter, each EKF iteration is also composed of two distinct phases, representing by two set of equations as below:

#### **The prediction process**

Predicted state estimate and its covariance through linearization of system dynamics and application of the prediction step of the Kalman filter

$$\mathbf{x}\_{k/k-1} = f\left(\mathbf{x}\_{k-1/k-1}, \mathbf{u}\_{k-1}\right) \tag{34}$$

$$\mathbf{P}\_{k/k-1} = \mathbf{F}\_{k-1} \mathbf{P}\_{k-1/k-1} \mathbf{F}\_{k-1}^T + \mathbf{Q}\_{k-1} \tag{35}$$

### **The measurement update**

82 Magnetic Sensors – Principles and Applications

T T

Typically, the two phases alternate, with the prediction advancing the state until the next

Kalman filter is optimal in either the least square sense or maximum likelihood sense if it is applied to linear dynamic system with zero mean, normally distributed noise. Kalman filter becomes not optimal when the system is nonlinear, or the noise is not normally distributed. EKF is the nonlinear version of the Kalman filter. EKF linearizes the dynamic system about the current mean and covariance. The estimate is suboptimal, i.e. it is an approximation of

In EKF, the state transition and measurement models may be non-linear differentiable

where system noise **w***k* is normally distributed with zero mean and covariance **Q***k* while measurement noise **v***k* is zero mean normally distributed with covariance **R***k*. The functions *f* and *h* can be used to estimate the current state and measurement from previous state and measurement respectively. However, the current covariance of system noise and measurement noise cannot be estimated from these two functions directly. Instead the Jacobians, matrices of partial derivatives, must be calculated. The state transition and

1

*k*

**H**

*f*

**x**

**x u**

*h*

**x**

**x**

Similar with Kalman filter, each EKF iteration is also composed of two distinct phases,

Predicted state estimate and its covariance through linearization of system dynamics and

*k*

**F**

1/ 1 1

, *kk k*

*k k*/ 1

scheduled measurement, and the update incorporating the measurement.

**P P KHP** *kk kk k k kk* / /1 / 1 (28)

*k kk k k kk k k* /1 /1 **K P H HP H R** (29)

**x xu w** *k kk k f* 11 1 , (30)

( ) *k kk* **z xv** *h* (31)

(32)

(33)

Accordingly, the updated (*a posteriori*) estimate covariance is:

where the optimal Kalman gain matrix is given by:

**6.3 Extended Kalman filter** 

the optimal estimate.

measurement matrices are defined as:

representing by two set of equations as below:

application of the prediction step of the Kalman filter

**The prediction process** 

functions:

Measurement residual is calculated through linearization of system dynamics

$$\mathbf{y}\_k = \mathbf{z}\_k - h\left(\mathbf{x}\_{k/k-1}\right) \tag{36}$$

The relevant covariance is

$$\mathbf{S}\_k = \mathbf{H}\_k \mathbf{P}\_{k/k-1} \mathbf{H}\_k^\top + \mathbf{R}\_k \tag{37}$$

The suboptimal Kalman gain is

$$\mathbf{K}\_k = \mathbf{P}\_{k/k-1} \mathbf{H}\_k^T \mathbf{S}\_k^{-1} \tag{38}$$

The state estimate is updated as

$$\mathbf{x}\_{k/k} = \mathbf{x}\_{k/k-1} + \mathbf{K}\_k \mathbf{y}\_k \tag{39}$$

The relevant estimate covariance is also updated as

$$\mathbf{P}\_{k/k} = (\mathbf{I} - \mathbf{K}\_k \mathbf{H}\_k)\mathbf{P}\_{k/k-1} \tag{40}$$

### **6.4 Application examples**

Kalman filter, sometimes with other techniques, have already been used for data fusion and positioning. Recently, Zhao & Wang (2011) use EKF for the fusion of data from inertial sensors, ultrasonic sensors, and magnetic sensor. A 3D magnetic sensor and a 3D accelerometer are combined to measure the gravity and the earth's magnetic field for the static orientation. A 3D gyroscope is used to obtain the dynamic orientation. The integral drift of accelerometer is periodically calibrated by an ultrasonic sensor. All these data are fused by EKF. The measurements are the position obtained by ultrasonic sensor and the orientation obtained by magnetic sensors and accelerometers. The states incorporated the position, velocity, and orientation. The experimental results demonstrated that the uncertainty of orientation and position is lowered.

In another research, Shen et al (2011) apply Kalman filter, together with Fast Orthogonal Search (FOS) in the 2D navigation for land vehicles. Kalman filter is used for data fusion of GPS and inertial sensors, i.e. the integration of GPS, IMU and vehicle built-in odometer. FOS is a nonlinear error identification technique. It is used to model non-stationary stochastic sensor errors and non-linear inertial errors in the study. The road test trajectories confirm that a module with both Kalman filtering and FOS technique outperforms that with Kalman filtering only.

It can be concluded that improvement of positioning accuracy will be possible if data from multiple sensors, including magnetic sensors, are fused by Kalman filter. Further improvement is achievable if Kalman filtering is combined with other techniques.

**5** 

*Japan* 

**Application of Magnetic Sensors to Nano-**

Takaya Inamori and Shinichi Nakasuka

*The University of Tokyo* 

**and Micro-Satellite Attitude Control Systems** 

These days, nano (1 kg – 10 kg ) - and micro (10 kg – 100 kg) – satellites, which are smaller than conventional large satellite, provide space access to broader range of satellite developers and attract interests as an application of the space developments because of shorter development period at smaller cost. Several new nano- and micro-satellite missions are proposed with sophisticated objectives such as remote sensing and observation of astronomical objects. In these advanced missions, some nano- and micro-satellites must meet strict attitude requirements for obtaining scientific data or high resolution Earth images. Example of these small satellites are nano remote sensing satellite PRISM, which should be stabilized to 0.7 deg/s (Inamori et al, 2011(a)), and nano astronomy satellite "Nano-JASMINE", which should be stabilized to 1 arcsec (Inamori et al, 2011(b)). Most of these small satellites have the strict constraint of power consumption, space, and weight. Therefore, magnetometers which are lightweight, reliable, and low power consumption sensors are used in the most of these small satellite missions as a sensor for an attitude determination system. In addition, most of these satellites use magnetometers for the attitude control systems with magnetic actuators to calculate required output torque. Furthermore, in some nano- and micro-satellite missions, a magnetic moment is estimated using magnetometers to compensate the magnetic moment and magnetic disturbance. In these small satellite missions, magnetometers play a more important role than conventional large satellites to achieve the attitude control. Although the magnetometers are useful for nano- and micro-satellite attitude control systems, these sensors which are not calibrated are not suitable for an accurate attitude control because of the measurement noises which are caused from magnetized objects and current loops in a satellite. For a precise attitude estimation, many satellites use heavier and higher power consumption sensor such as star truckers and gyro sensors, which are difficult to be assembled to some small satellites. To achieve the accurate attitude control in the small satellite missions, the magnetometers should be calibrated precisely for the accurate attitude control system. This chapter will present what is the requirement for magnetometers in the nano- and micro-satellite

missions, how these magnetometers are used in these small satellite missions.

In many nano- and micro-satellite missions, magnetometers are indispensable for attitude control systems and assembled to most of these satellites. In many cases, these small

**2. Magnetic sensors in nano- and micro-satellite missions** 

**1. Introduction** 
