-
Notifications
You must be signed in to change notification settings - Fork 146
Sensor Fusion
In PSMoveService, there are two mutually exclusive ways we may use the term 'sensor fusion'. First, we may use sensor fusion to combine readings from different sensors to estimate orientation. Second, we may use sensor fusion to combine readings from even more sensors to estimate both orientation and position (i.e. the pose).
When we first started PSMoveService, we used an orientation filter to do sensor fusion to estimate the orientation only and the position was pulled directly from the optical tracker. Now we use a pose filter to do sensor fusion to estimate the orientation and position together.
In both cases, we use the set of sensors in the inertial measurement unit (IMU). In the PSMove controller, the IMU has 3 sensors:
- Accelerometer - Measures the direction of gravity
- Magnetometer - Measures the direction of your local magnetic field (basically a 3d compass)
- Gyroscope - Measures the rate at which the controller is rotating about the controller local X, Y and Z axes.
TO DO: IMAGE DEPICTING EACH SENSOR AND THE XYZ ORIENTATION. ALSO SENSOR OFFSETS
And we also use an optical tracker (Optical-Tracker-Algorithms) to get a first estimate of the position.
TODO: SIMPLE IMAGE OF OPTICAL TRACKER
#Calculating Orientation (only)
PSMoveService originally calculated orientation in its own filter, independent of any position data. We still fall back to this method if no optical tracker is available.
There are a few ways you could compute orientation from an IMU, though they aren't all recommended.
You could track a controller's orientation just by adding up the deltas (i.e., integrating) from the Gyroscope but there are two problems with this:
- You need to start from a known orientation
- Errors in the gyro readings will accumulate and within a few seconds the orientation will be completely off
Another way to determine orientation is to use a sensor that measures some locally static field pointing in a known direction like Earth's gravity. For example, if you know that your controller is standing upright when your sensor says gravity is pointing down, the controller's local Y-Axis and you are currently measuring gravity pointing down the controller's local X-Axis - then the controller must be rotated 90 degrees. To measure the direction of gravity the PS Move uses a 3-Axis Accelerometer.
Measuring the direction of one field allows you to compute two rotation values, typically yaw and pitch. Measuring the direction of two different fields pointing in different directions allows you to compute all 3 rotation values: roll, pitch, and yaw.
Just as in the single field case, if you measure two independent fields in one known controller pose (i.e. calibrate the controller sitting upright) and then compare those to current readings you can compute orientation by determining the rotation you would need to align the calibration readings with the current readings. Using the PS Move's 3-Axis Magnetometer we get the second field direction we need from the Earth's magnetic field.
So then if we can completely compute the orientation from the accelerometer and the magnetometer, why do we care about the gyroscope? The answer is noise and bias. Both the accelerator and magnetometer give much "noisier" readings than the gyroscope does. If you compute orientation from just the accelerometer and the magnetometer you'll get a really jittery looking orientation.
If however we can compute some weighted blend of the orientation computed from the Accelerometer-Magnetometer derived orientation with the Gyroscope readings we can get the best of both worlds: smooth orientation changes without the drift. This process of combining information from multiple sensors is called "Sensor Fusion". There are many algorithms for doing Sensor Fusion. They vary in what kinds of sensors they use, how they deal with noise, whether they need calibration or not, and how computationally expensive they are to run.
The PSMoveService library uses a modified version of Madgwick's AHRS orientation filter which I adapted from his paper describing the filter. His original method and implementation require no calibration because the default magnetometer direction is supposed to converge on the correct value. In practice I've found that it never quite did this for me and the filter suffered from drift in many circumstances as a result. For increased reliability (at the cost of ease of use) we instead do a manual calibration of the magnetometer. This calibration process is outlined on the PSMove Magnetometer Calibration wiki page.
TODO: Also mention basic Magwick, complementary filter and Kalman filter
To calculate pose (orientation + position), we perform sensor fusion using all sensors in the IMU and the rough positional estimate from the optical tracker.
We want to estimate the state of the controller which comprises several variables. Our sensors don't directly provide all of these states, and the relationships are noisy.
A common way to calculate a cleaner estimate of an unobserved state using a set of noisy sensors is with a Kalman filter. In our case, the math governing how our states evolve over time and how our sensors relate to our states are non-linear, so we use a non-linear sigma-point Kalman filter (SPKF), specifically the square-root formulation of the Unscented Kalman filter. TODO: Links for the different types of KFs.
The system has 2 driving functions. First, the "process function" determines how the state x at time k-1 can lead to a state at time k (i.e., x_k). It is also assumed that there is some process noise v in the state transition. Second, the "observation function" determines how the underlying state can generate the observed sensor values in y. It is also assumed that the observation function has some noise n.
So we start with some state x_(k-1), run that through the process function to get x_k, then run that through the observation function to get expected sensor values ybar_k.
The difference between true measured sensor values y_k and expected sensor values ybar_k is called the innovation. We can use the innovation to refine our estimate of x_k. The degree to which the innovation is blended into the state estimate is determined by the Kalman gain K, which is a function of the covariance of the state and innovation.
Calculating the covariance matrices is rather complicated and there are many different ways to do it depending on whether the system is linear or not and other factors. Luckily for us, all of this has been worked out and various open source Kalman filtering packages are available.
While much of the code is taken care of, we have a few tasks:
- Define the state vector
- Define the process model (i.e., state transition function)
- Define the process noise
- Define the observation (sensor) vector
- Define the observation model (i.e., how state relates to sensor values)
- Define the observation noise
A minimal pose state vector includes:
- Position x/y/z
- Orientation
- Represented as a unit quaternion w/x/y/z
- 4 variables but only 3 degrees of freedom
But, for gaming purposes, and also to make some of the filtering work better, we add the following variables to our state vector:
- Linear velocity x/y/z
- Linear acceleration x/y/z
- Angular velocity x/y/z
For a total of 16 state variables.
The process function describes how we can predict a future state assuming no input. We need process models for every one of our state variables. For now I'll include the noise terms, even though they may be pulled out depending on the specific UKF implementation.
There are several ways to describe the rest of the process model. Note that orientation, represented in a quaternion, is updated via quaternion multiplication, but angular velocity is a 3-vector updated via addition.
new_position = old_position + old_velocity*dt + 0.5*old_acceleration*dt*dt + position_noise
new_velocity = old_velocity + old_acceleration*dt + velocity_noise
new_acceleration = old_acceleration + acceleration_noise
new_orientation = old_orientation * orientation_noise * delta_orientation_from_old_angular_velocity
new_angular_velocity = old_angular_velocity + angular_velocity_noise
There are a few ways to get delta_orientation_from_old_angular_velocity
rot_angle = mag(angular_velocity) * dt
rot_axis = angular_velocity / mag(angular_velocity)
delta_q = [cos(rot_angle/2) rot_axis*sin(rot_angle/2)]
omega_q = [old_ang_vel*0.5 0.0]
delta_q = dt*omega_q.conjugate()
There are a many things we could change. Among them:
- Add angular acceleration to the state vector
- assume angular acceleration is constant but angular velocity is changing
- PSMoveService no longer has to calculate diff(ang_vel) from the last 2 angular_velocity measurements to get angular acceleration.
- Compensate for offsets between individual sensors and the controller 0,0,0 point (centre of bulb)
- Add dynamics models or kinematic constraints if we assume the controller will always be clutched in someone's hand
- Not guaranteed and it's a lot of work to implement.
- Treat the accelerometer and gyroscope readings as inputs to the model (so far we have described a 0-input system)
- Account for delays in optical measurements relative to IMU measurements.
-
van der Merwe, Wan, Julier 2004
- Section IV.A
- A little bit different to what we want because here they pull the sensor readings and biases into the process model.
- delta_velocity = acceleration, where 'acceleration' is bias&noise&gravity corrected values directly from accelerometer.
- Also corrects for lever-arm if accelerometer is not in center-of-gravity.
- delta_orientation = -1/2 * OMEGA_gyro * orientation,
- where OMEGA_gyro = [0 g_p g_q g_r -g_p 0 -g_r g_q -g_q g_r 0 -g_p -g_r -g_q g_p 0]
- where g_ are bias&earth&noise-corrected gyroscope readings.
- Also update functions for sensor bias
-
Enayati 2013
- Section 3.4.3
-
Kraft 2003
- Section 2.2
- Crassidis and Markley, 2003
- Accelerometer x/y/z
- Gyroscope x/y/z
- Magnetometer x/y/z
- Optical tracker x/y/z
x_world_to_controller = orientation_quat.toConjugate().toMatrix()
accelerometer = x_world_to_controller * (linear_acceleration + gravity) + accelerometer_bias + accelerometer_noise
gyroscope = angular_velocity + gyroscope_bias + gyroscope_noise
magnetometer = x_world_to_controller * (local_magnetic_field) + magnetometer_bias + magnetometer_noise
optical_tracker = position + position noise.
For x_world_to_controller, see van der Merwe, Wan, Julier 2004 equation 36, or quaternion_conjugate followed by quaternion_matrix here.
For now, let's assume that the sensor bias is constant and properly captured in calibration variable, and that sensor scaling is properly accounted for in PSMoveService calibration. Thus we can drop the _bias terms from the above.
Position noise is anisotropic when using < 3 cameras (to-from camera is noisier than left-right-up-down).