Skip to content

Commit 5437732

Browse files
shujaatakshujaatak
authored
Merge pull request #2 from bolderflight/master
Update
2 parents 77c637b + add41ec commit 5437732

File tree

5 files changed

+471
-280
lines changed

5 files changed

+471
-280
lines changed

‎README.md

+15-34
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
uNav Attitude and Heading Reference System 7 State EKF Arduino Library.
33

44
# Description
5-
The uNav Attitude and Heading Reference System (AHRS) is a 7 state Extended Kalman Filter (EKF) to estimate attitude and heading from IMU data. The 7 states comprise a quaternion and three gyro biases. The algorithm was developed by Jung Soon Jang at Stanford University ([1,2](<#references>)) and this library was adapted from software developed and flight tested at the [University of Minnesota UAS Research Labs](http://www.uav.aem.umn.edu), where it has been used since 2006. uNav AHRS provides good estimates of attitude and heading without requiring a GPS. It uses gyro measurements to propogate the state; accelerometers are used as a measurement update on the pitch and roll channels and magnetometers as a measurement update on the yaw channel to constrain drift.
5+
The uNav Attitude and Heading Reference System (AHRS) is a 7 state Extended Kalman Filter (EKF) to estimate attitude and heading from IMU data. The 7 states comprise a quaternion and three gyro biases. It uses gyro measurements to propogate the state; accelerometers are used as a measurement update on the pitch and roll channels and magnetometers as a measurement update on the yaw channel to constrain drift.
66

77
# Usage
88

@@ -19,48 +19,34 @@ uNavAHRS Filter;
1919
```
2020

2121
### Setup Functions
22-
A sample rate divider for the magnetometer can be configured for situations where: the magnetometer is measured at a lower rate than the accelerometers and gyros or you would like the magnetometer measurement update to run at a lower rate than the rest of the filter. Additionally, the accelerometer covariance and heading angle covariance, as measured by the magnetometers, can be configured. By default, the magnetometer sample rate divider is set to 0 (i.e. magnetometers sampled at the same rate as the accelerometers and gyros), the accelerometer covariance is set to 0.96177249 (0.1g)^2 or (0.9807m/s/s)^2, the heading angle covariance is set to 0.014924 (7deg)^2 or (0.122173rad)^2.
23-
24-
**(optional) void setMagSrd(uint8_t magSRD)**
25-
This is an optional function to set the magnetometer sample rate. This is useful for situations where: the magnetometer is measured at a lower rate than the accelerometers and gyros or you would like the magnetometer measurement update to run at a lower rate than the rest of the AHRS filter. The rate is set by a sample rate divider, *uint8_t magSRD*. The rate is then given by:
26-
27-
*Magnetometer Rate = Filter Rate / (1 + magSRD)*
28-
29-
A sample rate divider of 0 means the magnetometer measurement update occurs every time the AHRS is updated. A sample rate divider of 1 is every other time and so on. A sample rate divider of 9 with an AHRS update rate of 100 Hz means the magnetometer measurement update would occur at 10 Hz. The following is an example of setting a SRD of 9.
30-
31-
```C++
32-
Filter.setMagSrd(9);
33-
```
34-
35-
**(optional) void setAccelCovariance(float cov)**
36-
This is an optional function to configure the accelerometer covariance, which is a measure of the accelerometer noise. It is important to note that the covariance is not normalized and should be set to the appropriate value for the application and environment. The units are given in (m/s/s)^2. If this function is not used, a default value of 0.96177249 (0.1g)^2 or (0.9807m/s/s)^2 is applied. The following is an example of setting a covariance of 3.84708996 (0.2g)^2 or (1.9614m/s/s)^2
22+
**(optional) void setInitializationDuration(uint32_t duration)**
23+
Statistical data are gathered from the IMU measurements for automatically setting up the filter. By default this initialization takes 2 minutes. Optionally, this function can be used to set initialization durations other than the default value. The input is initialization time in microseconds.
3724

3825
```C++
39-
Filter.setAccelCovariance(3.84708996f);
40-
```
41-
42-
**(optional) void setHeadingCovariance(float cov)**
43-
This is an optional function to configure the heading covariance, which is a measure of the magnetometer noise. It is important to note that the covariance is not normalized and should be set to the appropriate value for the application and environment. The units are given in (rad)^2. If this function is not used, a default value of 0.27415570336144 (30deg)^2 or (0.5235988rad)^2 is applied. The following is an example of setting a covariance of 0.122173 (7deg)^2 or (0.122173rad)^2.
44-
45-
```C++
46-
Filter.setHeadingCovariance(0.122173f);
26+
Filter.setInitializationDuration(60000000);
4727
```
4828

4929
### Data Collection Functions
5030

51-
**void update(float ias,float p,float q,float r,float ax,float ay,float az,float hx, float hy, float hz)** updates the filter with new IMU measurements, time updates propogate the state and measurement updates are made; the attitude and heading of the vehicle is updated. Inputs are:
31+
**bool update(float ias,float p,float q,float r,float ax,float ay,float az,float hx, float hy, float hz)** updates the filter with new IMU measurements. Inputs are:
5232

5333
* float p: gyro measurement in the x direction, units are rad/s.
5434
* float q: gyro measurement in the y direction, units are rad/s.
5535
* float r: gyro measurement in the z direction, units are rad/s.
56-
* float ax: accelerometer measurement in the x direction, units are m/s/s.
57-
* float ay: accelerometer measurement in the y direction, units are m/s/s.
58-
* float az: accelerometer measurement in the z direction, units are m/s/s.
36+
* float ax: accelerometer measurement in the x direction, units need to be consistant across all accelerometer measurements used
37+
* float ay: accelerometer measurement in the y direction, units need to be consistant across all accelerometer measurements used
38+
* float az: accelerometer measurement in the z direction, units need to be consistant across all accelerometer measurements used
5939
* float hx: magnetometer measurement in the x direction, units need to be consistant across all magnetometer measurements used.
6040
* float hy: magnetometer measurement in the y direction, units need to be consistant across all magnetometer measurements used.
6141
* float hz: magnetometer measurement in the z direction, units need to be consistant across all magnetometer measurements used.
6242

63-
Please note that all directional measurements (i.e. all measurements other than airspeed) need to be given in the [defined axis system](#axis-system).
43+
Please note that all measurements need to be given in the [defined axis system](#axis-system). Measurements need to be provided in integer rates of each other, which the gyro measurements always updated. The *update* function checks each set of measurements to see if they've been updated and uses the following logic:
44+
45+
* Gyro, accelerometer, and magnetometer measurements updated: filter time update, accelerometer and magnetometer measurement update.
46+
* Gyro and accelerometer measurements updated: filter time update, accelerometer measurement update.
47+
* Gyro measurements updated: filter time update.
48+
49+
The filter automatically initializes itself. Calls to *update* return false if the filter has not completed its initialization and return true after initialization is complete. The duration of initialization can be optionally set with the *setInitializationDuration* function, otherwise, the filter takes 2 minutes to initialize by default.
6450

6551
```C++
6652
// read the sensor
@@ -125,8 +111,3 @@ This library expects IMU data to be input in a defined axis system, which is sho
125111

126112
## Example List
127113
* **uNavAHRS-with-MPU9250**: demonstrates using this filter with an MPU-9250 IMU. *CalibrateMPU9250.ino* is used to calibrate the MPU-9250 IMU and store the calibration coefficients in EEPROM. *uNavAHRS_MPU9250.ino* uses the MPU-9250 IMU as measurement input to the uNav AHRS filter, which is run at a rate of 100 Hz.
128-
129-
# <a name="references">References
130-
131-
1. Jung Soon Jang, & Liccardo. (2006). Automation of Small UAVs using a Low Cost Mems Sensor and Embedded Computing Platform. 25th Digital Avionics Systems Conference, 2006 IEEE/AIAA, 1-9.
132-
2. Jung Soon Jang, & Liccardo. (2007). Small UAV Automation Using MEMS. Aerospace and Electronic Systems Magazine, IEEE, 22(5), 30-34.

‎examples/uNavAHRS-with-MPU9250/uNavAHRS_MPU9250/uNavAHRS_MPU9250.ino

+18-9
Original file line numberDiff line numberDiff line change
@@ -98,15 +98,24 @@ void loop() {
9898
// read the sensor
9999
Imu.readSensor();
100100
// update the filter
101-
Filter.update(Imu.getGyroX_rads(),Imu.getGyroY_rads(),Imu.getGyroZ_rads(),Imu.getAccelX_mss(),Imu.getAccelY_mss(),Imu.getAccelZ_mss(),Imu.getMagX_uT(),Imu.getMagY_uT(),Imu.getMagZ_uT());
102-
tstop = micros();
103-
Serial.print(Filter.getPitch_rad()*180.0f/PI);
104-
Serial.print("\t");
105-
Serial.print(Filter.getRoll_rad()*180.0f/PI);
106-
Serial.print("\t");
107-
Serial.print(Filter.getYaw_rad()*180.0f/PI);
108-
Serial.print("\t");
109-
Serial.println(tstop - tstart);
101+
if (Filter.update(Imu.getGyroX_rads(),Imu.getGyroY_rads(),Imu.getGyroZ_rads(),Imu.getAccelX_mss(),Imu.getAccelY_mss(),Imu.getAccelZ_mss(),Imu.getMagX_uT(),Imu.getMagY_uT(),Imu.getMagZ_uT())) {
102+
tstop = micros();
103+
Serial.print(Filter.getPitch_rad()*180.0f/PI);
104+
Serial.print("\t");
105+
Serial.print(Filter.getRoll_rad()*180.0f/PI);
106+
Serial.print("\t");
107+
Serial.print(Filter.getYaw_rad()*180.0f/PI);
108+
Serial.print("\t");
109+
Serial.print(Filter.getHeading_rad()*180.0f/PI);
110+
Serial.print("\t");
111+
Serial.print(Filter.getGyroBiasX_rads(),6);
112+
Serial.print("\t");
113+
Serial.print(Filter.getGyroBiasY_rads(),6);
114+
Serial.print("\t");
115+
Serial.print(Filter.getGyroBiasZ_rads(),6);
116+
Serial.print("\t");
117+
Serial.println(tstop - tstart);
118+
}
110119
}
111120
}
112121

‎keywords.txt

+1-3
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,5 @@
11
uNavAHRS KEYWORD1
2-
setMagSrd KEYWORD2
3-
setAccelCovariance KEYWORD2
4-
setHeadingCovariance KEYWORD2
2+
setInitializationDuration KEYWORD2
53
update KEYWORD2
64
getPitch_rad KEYWORD2
75
getRoll_rad KEYWORD2

0 commit comments

Comments
 (0)