You are on page 1of 6

Author

:
Title:
Location:
Date:
Email

Jiawei Wu
Doc13-Sensor Fusion Algorithm
Implementation - Part IV.Doc
Politecnico di Torino
27/02/15
Jiawei.wu@gmx.de

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

..........4  Kalman filter parameters ....................................1.............................3  Sensor parameters .......de Table of Contents 1..................... 5  ...................................................1....................................1.............................................................. 3  1..............................wu@gmx.......................  Simulation model .............1  Top-level structure modification .. 4  1...................... 2  1....2.1  Simulink parameters definition .......................  Reference .... 4  2..................................2  Observation block modification ...............1......................................1  Sensor measurement from Simulator ......................... 4  1............................................................................................. 2  1.............. 2  1..............................................Doc Politecnico di Torino 27/02/15 Jiawei........Part IV......................2............... 2  1......2  Simulink diagram of EKF.............................................................................................. 3  1.......2  Simulation environment parameters......................................................Author: Title: Location: Date: Email Jiawei Wu Doc13-Sensor Fusion Algorithm Implementation .....................................................

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.12e-5] [Tesla] 3 g 9. 1.wu@gmx. Meanwhile.de 1. one is for the structure.2 Simulation environment parameters This section will list the parameters which will be used for simulation.36e-7. It also includes two modifications.1 Sensor measurement from Simulator This section will list the sensor measurement data which have been obtained from Simulator.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. the point is to consider that how to relate the sensor measurement with our states. 1. print.-6. but for implementing Kalman filter. Especially for sensor measurement transition. Simulation model This document will present all the parameters which have been used in our blocks. 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. It is mainly obtained from sensor datasheet or previous test. the same measurement of vertical height from sonar and barometer.Part IV. So.Author: Title: Location: Date: Email Jiawei Wu Doc13-Sensor Fusion Algorithm Implementation .1.mat .de 2 . Name Value Unit 1 Tsim 5e-4 [s] 2 mag [2.wu@gmx. Jiawei.27e-5. No. I considered that. and variables for Kalman filter. and the more important one is for observation block equations. 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 . I could combine or integrate the same measurement of sensor while the same measurement exists. post and distribute to other people without the written consent of the author. modify.mat .-4.1. Before. 'Real' sensor measurement 1.mat .80665 [m/s^2] 4 P0_baro 101325 [Pa] 5 T0_baro 288. These data will be used to compute the model error by combining the ‘expected’ sensor measurement. it mainly contains the character of quadrotor and simulation environment properties.mat . we need to design the sensor model block.mat . no matter how many sensors we are using.Doc Politecnico di Torino 27/02/15 Jiawei. it will list all the parameters defined in Simulink block and code.1 Simulink parameters definition In this section. No. for instance. we only need to obtain noise less states which is for controller.mat . and the proper values. It is not allowed to copy.

Sensor character parameters 1.Jiawei Wu Doc13-Sensor Fusion Algorithm Implementation .1.1) [m/s^2] 3 A_sfcc_gyro eye(3) - 4 b_gyro zeros(3.5 [K/m] [J/(mol*K)] [Kg] 9 Tp_control 0.3 Sensor parameters This section will list the parameters defined for sensor model.43051e-4 1 1.wu@gmx. print.1) 0 0 0.1) [rad/s] 5 G_mag eye(3) - 6 7 8 9 b_mag b_baro b_sonar Q zeros(3.de Author: Title: Location: Date: Email 6 7 8 L_baro R_baro m -6. Name Value Unit 1 A_sfcc_acc eye(3) - 2 b_acc zeros(3. including the parameters for ADC and so on.1. modify.05861e-4 1. No.0e-1 1 1. Jiawei.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. 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.02 [s] 10 11 12 Ix Iy Iz 3.de 3 .058 0.Doc Politecnico di Torino 27/02/15 Jiawei.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.4 Kalman filter parameters This section will list the parameters which are used in Kalman filter implementation.21e-2 3.10571e-4 8.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. Simulation environment paramters 1. It is not allowed to copy. post and distribute to other people without the written consent of the author.5e-3 287. No.wu@gmx.Part IV.0e-2 2e-3 2e-3 2e-2 1.10352e-4 2.

however. post and distribute to other people without the written consent of the author.004*eye(14) - Jiawei Wu Doc13-Sensor Fusion Algorithm Implementation .Part IV. 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.2. 1. reduced the nested blocks into the following structure. meanwhile. Figure 1. Top-leve view of algorithm I added the radian to degree convert gain after the measurement of gyroscope. modify. print.2 Simulink diagram of EKF Based on detailed analysis in Doc12. using one selector to select the required information from barometer sensor.Author: Title: Location: Date: Email 10 R 0.1 Top-level structure modification I have simplified the top-level view of the algorithm. the following part will list the modification.2.Doc Politecnico di Torino 27/02/15 Jiawei.de Measurement H noise variance Table 4.wu@gmx. Jiawei. It is not allowed to copy. 1. I have modified part of the Simulink logic and diagram. The output of this block is the ‘expected’ sensor measurement.de .2 Observation block modification I modified the equations. in order to minimize the model errors based on applying the predicted states. Parameters defined for Kalman filter 1. we need to reduce the errors by combining the ‘real’ sensor measurement from simulator.wu@gmx.

Doc Politecnico di Torino 27/02/15 Jiawei. Jiawei. It is not allowed to copy. post and distribute to other people without the written consent of the author. which is related to twelve states. Expected sensor measurement The output of this block is with dimension 14 12 .wu@gmx.wu@gmx. modify. 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.de .Part IV.de Figure 2. 2.Author: Title: Location: Date: Email Jiawei Wu Doc13-Sensor Fusion Algorithm Implementation . print.