Implement a Kalman filter to smooth data from deviceOrientation API

Question

I'm trying to smooth the data I'm getting from the deviceOrientation API to make a Google Cardboard application in the browser.

I'm piping the accelerometer data straight into the ThreeJs camera rotation but we're getting a lot of noise on the signal which is causing the view to judder.

Someone suggested a Kalman filter as the best way to approach smoothing signal processing noise and I found this simple Javascript library on gitHub

https://github.com/itamarwe/kalman

However its really light on the documentation.

I understand that I need to create a Kalman model by providing a Vector and 3 Matrices as arguments and then update the model, again with a vector and matrices as arguments over a time frame.

I also understand that a Kalman filter equation has several distinct parts: the current estimated position, the Kalman gain value, the current reading from the orientation API and the previous estimated position.

I can see that a point in 3D space can be described as a Vector so any of the position values, such as an estimated position, or the current reading can be a Vector.

What I don't understand is how these parts could be translated into Matrices to form the arguments for the Javascript library.

Answer 1

Well, I wrote the abhorrently documented library a couple of years ago. If there's interest I'm definitely willing to upgrade it, improve the documentation and write tests.

Let me shortly explain what are all the different matrices and vectors and how they should be derived:

x - this is the vector that you try to estimate. In your case, it's probably the 3 angular accelerations.

P - is the covariance matrix of the estimation, meaning the uncertainty of the estimation. It is also estimated in each step of the Kalman filter along with x.

F - describes how X develops according to the model. Generally, the model is x[k] = Fx[k-1]+w[k]. In your case, F might be the identity matrix, if you expect the angular acceleration to be relatively smooth, or the zero matrix, if you expect the angular acceleration to be completely unpredictable. In any case, w would represent how much you expect the acceleration to change from step to step.

w - describes the process noise, meaning, how much does the model diverge from the "perfect" model. It is defined as a zero mean multivariate normal distribution with covariance matrix Q.

All the variables above define your model, meaning what you are trying to estimate. In the next part, we talk about the model of the observation - what you measure in order to estimate your model.

z - this is what you measure. In your case, since you are using the accelerometers, you are measuring what you are also estimating. It will be the angular accelerations.

H - describes the relation between your model and the observation. z[k]=H[k]x[k]+v[k]. In your case, it is the identity matrix.

v - is the measurement noise and is assumed to be zero mean Gaussian white noise with covariance R[k]. Here you need to measure how noisy are the accelerometers, and calculate the noise covariance matrix.

To summarize, the steps to use the Kalman filter:

1. Determine x[0] and P[0] - the initial state of your model, and the initial estimation of how accurately you know x[0].
2. Determine F based on your model and how it develops from step to step.
3. Determine Q based on the stochastic nature of your model.
4. Determine H based on the relation between what you measure and what you want to estimate (between the model and the measurement).
5. Determine R based on the measurement noise. How noisy is your measurement.

Then, with every new observation, you can update the model state estimation using the Kalman filter, and have an optimal estimation of the state of the model(x[k]), and of the accuracy of that estimation(P[k]).

Answer 2

var acc = {
 x:0,
 y:0,
 z:0
};

var count = 0;

if (window.DeviceOrientationEvent) {
  window.addEventListener('deviceorientation', getDeviceRotation, false);
}else{
  $(".accelerometer").html("NOT SUPPORTED")
}

var x_0 = $V([acc.x, acc.y, acc.z]); //vector. Initial accelerometer values

//P prior knowledge of state
var P_0 = $M([
              [1,0,0],
              [0,1,0],
              [0,0,1]
            ]); //identity matrix. Initial covariance. Set to 1
var F_k = $M([
              [1,0,0],
              [0,1,0],
              [0,0,1]
            ]); //identity matrix. How change to model is applied. Set to 1
var Q_k = $M([
              [0,0,0],
              [0,0,0],
              [0,0,0]
            ]); //empty matrix. Noise in system is zero

var KM = new KalmanModel(x_0,P_0,F_k,Q_k);

var z_k = $V([acc.x, acc.y, acc.z]); //Updated accelerometer values
var H_k = $M([
              [1,0,0],
              [0,1,0],
              [0,0,1]
            ]); //identity matrix. Describes relationship between model and observation
var R_k = $M([
              [2,0,0],
              [0,2,0],
              [0,0,2]
            ]); //2x Scalar matrix. Describes noise from sensor. Set to 2 to begin
var KO = new KalmanObservation(z_k,H_k,R_k);

//each 1/10th second take new reading from accelerometer to update
var getNewPos = window.setInterval(function(){

    KO.z_k = $V([acc.x, acc.y, acc.z]); //vector to be new reading from x, y, z
    KM.update(KO);

    $(".kalman-result").html(" x:" +KM.x_k.elements[0]+", y:" +KM.x_k.elements[1]+", z:" +KM.x_k.elements[2]);
    $(".difference").html(" x:" +(acc.x-KM.x_k.elements[0])+", y:" +(acc.y-KM.x_k.elements[1])+", z:" +(acc.z-KM.x_k.elements[2]))


}, 100);

 //read event data from device
function getDeviceRotation(evt){

    // gamma is the left-to-right tilt in degrees, where right is positive
    // beta is the front-to-back tilt in degrees, where front is positive
    // alpha is the compass direction the device is facing in degrees
    acc.x = evt.alpha;
    acc.y = evt.beta;
    acc.z = evt.gamma; 
    $(".accelerometer").html(" x:" +acc.x+", y:" +acc.y+", z:" +acc.z);
}

Here is a demo page showing my results

http://cardboard-hand.herokuapp.com/kalman.html

I've set sensor noise to a 2 scalar matrix for now to see if the Kalman was doing its thing but we have noticed the sensor has greater variance in the x axis when the phone is lying flat on the table. We think this might be an issue with Gimbal lock. We haven't tested but its possible the variance changes in each axis depending on the orientation of the device.