Author:
Title:
Location:
Date:
Email
Jiawei Wu
Doc13-Sensor Fusion Algorithm
Implementation - Part [Link]
Politecnico di Torino
27/02/15
[Link]@[Link]
Document History
Version
Number
0.1
Date
Updated
Dec-2-2014
0.2
Dec-3-2014
Jiawei Wu
0.3
0.4
0.5
0.6
Dec-4-2014
Dec-5-2014
Dec-7-2014
Dec-10-2014
Jiawei Wu
Jiawei Wu
Jiawei Wu
Jiawei Wu
Author
Brief Description of Changes
Jiawei Wu
Inital from tempate
Add section 1.1
Add sub-section 1.1.1, 1.1.2, 1.1.3, 1.1.4
Add section 1.2.1
Add section 1.2.2
Complete
Update the Table 3
Author:
Title:
Location:
Date:
Email
Jiawei Wu
Doc13-Sensor Fusion Algorithm
Implementation - Part [Link]
Politecnico di Torino
27/02/15
[Link]@[Link]
Table of Contents
1.
Simulation model ............................................................................................................... 2
1.1 Simulink parameters definition ................................................................................... 2
1.1.1 Sensor measurement from Simulator ................................................................... 2
1.1.2 Simulation environment parameters..................................................................... 2
1.1.3 Sensor parameters ................................................................................................ 3
1.1.4 Kalman filter parameters ...................................................................................... 3
1.2 Simulink diagram of EKF............................................................................................ 4
1.2.1 Top-level structure modification .......................................................................... 4
1.2.2 Observation block modification ........................................................................... 4
2. Reference ............................................................................................................................ 5
Author:
Title:
Location:
Date:
Email
Jiawei Wu
Doc13-Sensor Fusion Algorithm
Implementation - Part [Link]
Politecnico di Torino
27/02/15
[Link]@[Link]
1. Simulation model
This document will present all the parameters which have been used in our blocks, and the
proper values. Especially for sensor measurement transition, and variables for Kalman filter.
It also includes two modifications, one is for the structure, and the more important one is for
observation block equations. Before, I considered that, I could combine or integrate the same
measurement of sensor while the same measurement exists, for instance, the same
measurement of vertical height from sonar and barometer, but for implementing Kalman
filter, we only need to obtain noise less states which is for controller. So, no matter how many
sensors we are using, the point is to consider that how to relate the sensor measurement with
our states. Meanwhile, we need to design the sensor model block, by setting the proper value
of sensor characteristics which can be obtained from sensor calibration. Then we apply the
EKF technique to obtain the predicted states.
1.1 Simulink parameters definition
In this section, it will list all the parameters defined in Simulink block and code. It is mainly
obtained from sensor datasheet or previous test.
1.1.1 Sensor measurement from Simulator
This section will list the sensor measurement data which have been obtained from
Simulator. These data will be used to compute the model error by combining the expected
sensor measurement.
No.
Source
1 Accelerometer
2
Gyroscope
3 Magnetometer
4
Barometer
5
GPS
6
Sonar
7
Controller
Name
acc_sensor
gyro_sensor
mag_sensor
baro_sensor
gps_sensor
sonar_sensor
control_U
Data type
.mat
.mat
.mat
.mat
.mat
.mat
.mat
Notes
Acceleration along three axis
Angular velocity along three axis
Magnetic field along three axis
Pressure in vertical
Position in 3D
Measured distance
Control variable
Table 1. 'Real' sensor measurement
1.1.2 Simulation environment parameters
This section will list the parameters which will be used for simulation, it mainly contains
the character of quadrotor and simulation environment properties.
No.
Name
Value
Unit
Tsim
5e-4
[s]
mag
[2.27e-5;-6.36e-7;-4.12e-5]
[Tesla]
9.80665
[m/s^2]
P0_baro
101325
[Pa]
T0_baro
288.15
[K]
Notes
Simulation time for
simulator
Magnetic field vector
Earth surface
gravitational
acceleration
Sea level standard
atmospheric pressure
Sea level standard
Last modified at: 27/02/2015 21:21:49
By author: Jiawei Wu
This document can only be used by Space and Precision Automatic research group. It is not allowed to copy,
modify, print, post and distribute to other people without the written consent of the author.
[Link]@[Link]
Jiawei Wu
Doc13-Sensor Fusion Algorithm
Implementation - Part [Link]
Politecnico di Torino
27/02/15
[Link]@[Link]
Author:
Title:
Location:
Date:
Email
6
7
8
L_baro
R_baro
m
-6.5e-3
287.058
0.5
[K/m]
[J/(mol*K)]
[Kg]
Tp_control
0.02
[s]
10
11
12
Ix
Iy
Iz
3.21e-2
3.28e-2
6.15e-2
[Kg*m^2]
[Kg*m^2]
[Kg*m^2]
temperature
Temperature lapse rate
Universal gas constant
Mass of quadrotor
Sampling time of
controller
Inertia matrix element
Inertia matrix element
Inertia matrix element
Table 2. Simulation environment paramters
1.1.3 Sensor parameters
This section will list the parameters defined for sensor model, including the parameters for
ADC and so on.
No.
1
2
3
4
5
6
7
8
9
10
11
12
13
Name
rho_acc
rho_gyro
rho_mag
rho_baro
rho_gps
rho_sonar
Tp_acc
Tp_gyro
Tp_mag
Tp_baro
Tp_gps
Tp_sonar
Naverage
Value
6.10352e-4
2.10571e-4
8.05861e-4
1.43051e-4
1
1.0e-2
2e-3
2e-3
2e-2
1.0e-1
1
1.0e-1
10
Unit
[m/s^2/LSB]
[rad/s/LSB]
[V/LSB]
[hpa/LSB]
[m/LSB]
[m/LSB]
[s]
[s]
[s]
[s]
[s]
[s]
-
Notes
ADC gain for accelerometer
ADC gain for gyroscope
ADC gain for magnetometer
ADC gain for barometer
ADC gain for GPS
ADC gain for sonar
Sampling time for accelerometer
Sampling time for gyroscope
Sampling time for magnetometer
Sampling time for barometer
Sampling time for GPS
Sampling time for sonar
Used for mean filter
Table 3. Sensor character parameters
1.1.4 Kalman filter parameters
This section will list the parameters which are used in Kalman filter implementation.
No.
Name
Value
Unit
A_sfcc_acc
eye(3)
b_acc
zeros(3,1)
[m/s^2]
A_sfcc_gyro
eye(3)
b_gyro
zeros(3,1)
[rad/s]
G_mag
eye(3)
6
7
8
9
b_mag
b_baro
b_sonar
Q
zeros(3,1)
0
0
0.005*eye(12)
[Tesla]
[hPa]
[m]
-
Notes
Misalignment and scale factor
for accelerometer
Bias vector for accelerometer
Misalignment and scale factor
for gyroscope
Bias vector for gyroscope
Coefficient matrix for
magnetometer
Bias vector for magnetometer
Bias for barometer
Bias for sonar
Process F noise variance
Last modified at: 27/02/2015 21:21:49
By author: Jiawei Wu
This document can only be used by Space and Precision Automatic research group. It is not allowed to copy,
modify, print, post and distribute to other people without the written consent of the author.
[Link]@[Link]
Author:
Title:
Location:
Date:
Email
10
0.004*eye(14)
Jiawei Wu
Doc13-Sensor Fusion Algorithm
Implementation - Part [Link]
Politecnico di Torino
27/02/15
[Link]@[Link]
Measurement H noise variance
Table 4. Parameters defined for Kalman filter
1.2 Simulink diagram of EKF
Based on detailed analysis in Doc12, I have modified part of the Simulink logic and
diagram, the following part will list the modification.
1.2.1 Top-level structure modification
I have simplified the top-level view of the algorithm, reduced the nested blocks into the
following structure.
Figure 1. Top-leve view of algorithm
I added the radian to degree convert gain after the measurement of gyroscope, meanwhile,
using one selector to select the required information from barometer sensor.
1.2.2 Observation block modification
I modified the equations, in order to minimize the model errors based on applying the
predicted states. The output of this block is the expected sensor measurement, however, we
need to reduce the errors by combining the real sensor measurement from simulator. The
block diagram is as following:
4
Last modified at: 27/02/2015 21:21:49
By author: Jiawei Wu
This document can only be used by Space and Precision Automatic research group. It is not allowed to copy,
modify, print, post and distribute to other people without the written consent of the author.
[Link]@[Link]
Author:
Title:
Location:
Date:
Email
Jiawei Wu
Doc13-Sensor Fusion Algorithm
Implementation - Part [Link]
Politecnico di Torino
27/02/15
[Link]@[Link]
Figure 2. Expected sensor measurement
The output of this block is with dimension 14 12 , which is related to twelve states.
2. Reference
5
Last modified at: 27/02/2015 21:21:49
By author: Jiawei Wu
This document can only be used by Space and Precision Automatic research group. It is not allowed to copy,
modify, print, post and distribute to other people without the written consent of the author.
[Link]@[Link]