New to the forums.. questions about AQ6 GPS+INS fusion

Info and discussion about the original AQ v6 flight controller

New to the forums.. questions about AQ6 GPS+INS fusion

Postby aharmon » Tue Apr 22, 2014 2:27 am

Hello,

I am new to the multi-rotor world, and interested in learning more about the GPS-INS sensor fusion employed by the AQ6 FC. I hope I'm not violating your forum policies by asking where to find information, but I had no luck using the search utilities on the website..

_____________________________________________________________________________________________

Could someone please point me to any resources/ documentation where I can read about the sensor fusion that the AQ6 flight controller implements to integrate the UBLOX LEA-6T and DIMU?

_____________________________________________________________________________________________

Background:

While I am new to multi-rotors and the RC world, I eat nonlinear filters for breakfast, and I have several years of experience writing filters for GPS-INS state estimation.

I discovered AutoQuad while poking around online. I have been developing algorithms for multi-rotor control as a hobby, doing simulations and trade studies, getting ready to buy hardware. For me, the fun is in the sensor fusion algorithm development and the nonlinear control design. I don't want to just drop a control board on a frame and have something that flies; the fun part for me is going through the systems engineering activities of developing performance metrics and error budgets, and then writing estimators, algorithms, and controllers to meet the specified design criteria. E.g. I want to design a stochastic linear quadratic controller by modeling the moments and products of inertia of the airframe, characterizing the motor inertia, damping, and engine thrust, and placing the PVA eigenvalues where I want them.

I'm excited about the AQ6 platform because it's open-source firmware will allow me to play with the parts of the system that interest me (sensor fusion and control) without spending hours and hours on a ground-up hardware design (a huge win!). I can't wait to get started!
aharmon
 
Posts: 2
Joined: Tue Apr 22, 2014 1:42 am

Re: New to the forums.. questions about AQ6 GPS+INS fusion

Postby bn999 » Tue Apr 22, 2014 12:15 pm

Howdy. The state estimate filter that I designed for AQ was originally based on the work of Rudolph van der Merwe. A most useful document is his 2004 PhD thesis "Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models" and can be found here:

http://www.cse.ogi.edu/~rudmerwe/pubs/pdf/rvdmerwe_phd_thesis.pdf

There is no formal documentation for the AQ implementation other than the source code itself, which is relatively well commented. Most of the work went into making the UKF fit into the computational constraints of the MCU. The rest was determining the variance and noise parameters that hold everything together.

On the control side, we have developed a quaternion based adaptive attitude control system we call Quatos. Although it is not generally available quite yet, it may soon be.
bn999
 
Posts: 1559
Joined: Thu Jun 21, 2012 11:40 pm

Re: New to the forums.. questions about AQ6 GPS+INS fusion

Postby kinderkram » Tue Apr 22, 2014 1:16 pm

Welcome to AutoQuad, aharmon!

Sounds like the AQ will be the ideal playground for you - Im pretty sure you´ll find the Quatos quite interesting.

Norbert
kinderkram
 
Posts: 2911
Joined: Fri Jun 22, 2012 7:47 am

Re: New to the forums.. questions about AQ6 GPS+INS fusion

Postby aharmon » Wed Apr 23, 2014 5:49 am

Hi Bill, Thanks for your reply!

First, I want to say well done! I spent some time looking at the source code, and I'm impressed by what I see! I'm very excited to see that you're implementing a square-root central-difference UKF; That's my go-to nonlinear filter for systems that can maintain unimodal pdf's through system nonlinearities. I was also wonderfully surprised to see that your filter has a measurement model for optical flow measurements! That's very cool (and something I was planning to implement myself).

Do you mind if I ask a few clarifying questions about the code? I realize that I have asked many, many questions here. I've got several more to ask, but I'm going to stop here because I've already asked too many. Answer the ones that you want ;) I'd be surprised if I'm the first forum member to ask for an explicit definition of the coordinate frames and stochastic system model that you use.. seems like this is prerequisite knowledge for anyone who wants to drill down into any part of the nav code..

Could you please explicitly define the coordinate frames that you use?
  • What is the body-frame axis convention with respect to the AQ board?
  • What are you using for the global reference navigation frame? WGS-84? Earth-Centered Earth-Fixed?
  • I see that the filter runs in a topocentric NED frame. Does the origin of this topocentric frame move with the geodetic position of the body frame, or is the origin fixed with respect to Earth axes for a given flight?

Attitude Parameterization:
  • Quaternion definition: Is the free parameter (i.e. cos(theta)) element number 1 or element number 4 in your definition of the quaternion? I can't tell from the way you have coded navUkfQuatToMatrix().
  • Does the quaternion define the rotation from the NED frame to the body frame, or the rotation from the body frame to the NED frame? Another way to ask this question is to say, is the unit vector for the generalized rotation vector coordinitized in the body frame, or in the NED frame?
  • There are 12 unique Euler angle sequences that can be used to describe a generalized rotation. Can you explicitly define the convention you are using for the Euler angles? e.g. C_body2ned = Rot1{ roll } * Rot2{ pitch } * Rot3{ yaw }, where Rot1 corresponds to a rotation about the [1; 0; 0] vector coordinitized in body frame axes, Rot2 corresponds to a rotation about the [0; 1; 0] vector coordinitized in body frame axes, etc.

SRCDUKF States, System Equations, and Measurement Models:
  • I see that the state vector is topo velocity, topo position, accel bias, gyro bias, attitude quaternion, and pressure altitude. Is there a reason that you choose not to include the inertial sensor scale factor and misalignment states? (I'm assuming it's because the computational complexity grows as O(N^2).) Merely as a comment, I assert that by inspection, you can notice that the residual scale/ misalignment errors from calibration will get lumped into the estimates of the inertial sensor biases. This may require you to over-inflate the process noise elements for the accel and gyro bias states, which reduces both state observability and filter performance. These states are, of course, trajectory dependent. While they may be trivial for hovering flight, they are very important if the airframe experiences aggressive vehicle maneuvers. Has anyone on the AQ team pursued a filter for kinematic alignment? The states should be reasonably well observable if you fly the right trajectories and use integrated velocity updates.
  • I couldn't locate anything that resembled the inertial navigation equations in the source code.. do you use the IMU measurements to drive your process noise model, or do you mechanize an INS somewhere here that I'm not seeing?
  • You have the oh-so-awesome UBLOX LEA-6T on this flight control board (awesome, because it gives you access to the raw data and timing outputs), but you use the GPS position and velocity measurements instead of the pseudorange and doppler observables. Do you have future plans to do GPS-INS fusion in the range domain (i.e. tightly-coupled) instead of loosely fusing position and velocity in the topo frame?
  • Do you use one of the timing sync outputs of the UBLOX to command the sample clock for the i2c bus? I couldn't find that anywhere.
  • Am I correct that you model the inertial sensor biases as random walk (Brownian motion) stochastic processes? Why did you choose this versus a time-correlated bias model?


As an aside, I'm potentially interested in contributing some AQ documentation once I understand what you're doing, to enable other people like myself to play with the code that employs the sensor fusion.

Cheers!
aharmon
 
Posts: 2
Joined: Tue Apr 22, 2014 1:42 am

Re: New to the forums.. questions about AQ6 GPS+INS fusion

Postby bn999 » Wed Apr 23, 2014 1:20 pm

aharmon wrote:[*]What is the body-frame axis convention with respect to the AQ board?

Facing north, level: X = North, Y = East, Z = Down

aharmon wrote:[*]What are you using for the global reference navigation frame? WGS-84? Earth-Centered Earth-Fixed?

Uses the UBLOX default, which I think is WGS-84.

aharmon wrote:[*]I see that the filter runs in a topocentric NED frame. Does the origin of this topocentric frame move with the geodetic position of the body frame, or is the origin fixed with respect to Earth axes for a given flight?[/list]

In order to have state estimates' domain kept within the bounds of the precision offered by a single float type to keep numeric stability, the origin moves with the waypoint. Every time the craft is asked to come out of manual mode or target WP change, the target position is defined as 0,0 and position states are updated to reflect this.

aharmon wrote:[*] Quaternion definition: Is the free parameter (i.e. cos(theta)) element number 1 or element number 4 in your definition of the quaternion? I can't tell from the way you have coded navUkfQuatToMatrix().

Postion # 1.

aharmon wrote:[*] Does the quaternion define the rotation from the NED frame to the body frame, or the rotation from the body frame to the NED frame? Another way to ask this question is to say, is the unit vector for the generalized rotation vector coordinitized in the body frame, or in the NED frame?

Frame to world.

aharmon wrote:[*] There are 12 unique Euler angle sequences that can be used to describe a generalized rotation. Can you explicitly define the convention you are using for the Euler angles? e.g. C_body2ned = Rot1{ roll } * Rot2{ pitch } * Rot3{ yaw }, where Rot1 corresponds to a rotation about the [1; 0; 0] vector coordinitized in body frame axes, Rot2 corresponds to a rotation about the [0; 1; 0] vector coordinitized in body frame axes, etc.[/list]

I think there are various functions which illustrate the sequence. See for instance navUkfQuatToMatrix() or navUkfMatrixExtractEuler() in nav_ukf.c

aharmon wrote:[*] I see that the state vector is topo velocity, topo position, accel bias, gyro bias, attitude quaternion, and pressure altitude. Is there a reason that you choose not to include the inertial sensor scale factor and misalignment states? (I'm assuming it's because the computational complexity grows as O(N^2).)

In a system such as this, you are always fighting for resources. I have boiled things down to the least number of states necessary to perform up to the level of the quality of the observation data. Adding more might make things slightly better, but (in my opinion so far) it's not worth the cost.

Keep in mind that process and measurement noise as added as additional estimation states, so the actual state count is significantly larger than 17. Given this conservative approach, there is still a little bit of headroom available. If a few additional states are found to be useful (perhaps vision related) they could likely be accommodated.

aharmon wrote:Merely as a comment, I assert that by inspection, you can notice that the residual scale/ misalignment errors from calibration will get lumped into the estimates of the inertial sensor biases. This may require you to over-inflate the process noise elements for the accel and gyro bias states, which reduces both state observability and filter performance. These states are, of course, trajectory dependent. While they may be trivial for hovering flight, they are very important if the airframe experiences aggressive vehicle maneuvers. Has anyone on the AQ team pursued a filter for kinematic alignment? The states should be reasonably well observable if you fly the right trajectories and use integrated velocity updates.

Not in the air. But, a similar idea is used on the ground to calibrate all sensors including scale, bias, axis misalignment and how they change over temperature.

aharmon wrote:[*] I couldn't locate anything that resembled the inertial navigation equations in the source code.. do you use the IMU measurements to drive your process noise model, or do you mechanize an INS somewhere here that I'm not seeing?

The inertial update is done in the filter itself as a byproduct of time-step covariance estimation in classic UKF form. Take a look at navUkfTimeUpdate() in nav_ukf.c.

aharmon wrote:[*] You have the oh-so-awesome UBLOX LEA-6T on this flight control board (awesome, because it gives you access to the raw data and timing outputs), but you use the GPS position and velocity measurements instead of the pseudorange and doppler observables. Do you have future plans to do GPS-INS fusion in the range domain (i.e. tightly-coupled) instead of loosely fusing position and velocity in the topo frame?

That would be a nobel project for sure. Sadly it will probably not be possible to implement on the MCU due to the required computational resources. However, a more powerful co-processor would likely have enough horsepower to do this in real-time. Something like the Gumstix SoC. Steps have already been taken along these lines.

aharmon wrote:[*] Do you use one of the timing sync outputs of the UBLOX to command the sample clock for the i2c bus? I couldn't find that anywhere.

Actually, the hardware is capable and the software has been written to take advantage of the precise GNSS timing available. Long story, but I had some hardware issues with early releases of the MCU which forced me to turn off this feature. I've never gotten back around to it. On the TODO list.

aharmon wrote:[*] Am I correct that you model the inertial sensor biases as random walk (Brownian motion) stochastic processes? Why did you choose this versus a time-correlated bias model?[/list]

Simplicity mostly. I'm not sure that there much to gain by the added complexity of another model, but that's just a hunch.

aharmon wrote:As an aside, I'm potentially interested in contributing some AQ documentation once I understand what you're doing, to enable other people like myself to play with the code that employs the sensor fusion.


Love to see it. If you'd rather, PM me and we can discuss detail more easily. Unless there are others who are interested in this boring stuff :)
bn999
 
Posts: 1559
Joined: Thu Jun 21, 2012 11:40 pm

Re: New to the forums.. questions about AQ6 GPS+INS fusion

Postby sandmen » Wed Apr 23, 2014 2:10 pm

If it's possible, keep this thread here.
sandmen
 
Posts: 997
Joined: Fri Jun 22, 2012 7:25 am

Re: New to the forums.. questions about AQ6 GPS+INS fusion

Postby phynix » Wed Apr 23, 2014 4:30 pm

I would also like to follow (quietly) your explanations / discussions, even so I just started reading through the code.
phynix
 
Posts: 73
Joined: Mon Feb 11, 2013 11:03 pm

Re: New to the forums.. questions about AQ6 GPS+INS fusion

Postby srinath » Wed Apr 23, 2014 5:01 pm

I would like to work on the ukf documentation too. It also estimates GPS latency.
I started the ukf documentation about 2 years ago, but left it midway. Would be good to pick up where I left off
srinath
 
Posts: 1028
Joined: Mon Jun 25, 2012 5:47 pm

Re: New to the forums.. questions about AQ6 GPS+INS fusion

Postby Max » Wed Apr 23, 2014 9:20 pm

Excellent info here, please keep. :)

-Max
Max
 
Posts: 2814
Joined: Mon Aug 13, 2012 9:45 pm
Location: Near Ithaca, NY, USA

Re: New to the forums.. questions about AQ6 GPS+INS fusion

Postby chestnut » Thu Apr 24, 2014 5:07 am

I love UKF very much, and I have been researching UKF for 3 years, but still not really understand.
chestnut
 
Posts: 71
Joined: Wed Oct 23, 2013 3:45 am
Location: VietNam

Next

Return to AutoQuad 6 Flight Controller

Who is online

Users browsing this forum: No registered users and 5 guests

cron