-- import "github.com/shantanubhadoria/go-kalmanfilter/kalmanfilter"
Package kalmanfilter implements Kalman Filter(Linear Quadratic Estimation) support for Go language
Source and Bug reports at https://github.com/shantanubhadoria/go-kalmanfilter
package main
import (
"fmt"
"time"
"github.com/shantanubhadoria/go-kalmanfilter/"
)
myFilterData = new(kalmanfilter.FilterData)
var oldTime time.Time = time.Now()
for {
stateReading := float64(getStateSensorReading()) // in units X
deltaReading := float64(getDeltaSensorReading()) // in unit X per nanosecond
var newTime time.Time = time.Now()
var duration Duration = newTime.Sub(oldTime)
oldTime = newTime
newState := myFilterData.Update(stateReading, deltaReading, int64(duration/time.Nanosecond))
fmt.Println(newState)
}
The Kalman filter(https://en.wikipedia.org/wiki/Kalman_filter), also known as linear quadratic estimation (LQE), is an algorithm that uses a series of measurements observed over time, containing noise (random variations) and other inaccuracies, and produces estimates of unknown variables that tend to be more precise than those based on a single measurement alone.
Algorithm is recursive, which means it takes the output of its previous calculations as a factor in calculating the next step which improves its accuracy over time. The key to Kalman filters are two sensors with different kind of accuracy issues in each. Sensor A or the state sensor might give inaccurate values for a measurement on the whole but it doesn't drift. Sensor B or delta sensor gives much more accurate rate of change in value(or delta) but it drifts over time due to its small inaccuracies as it only measures rate of change and not the actual value. Kalman filter uses this knowledge to fuse results from both sensors to give a state value which is more accurate than state value received from any of these filters alone.
An example of application for this is calculating orientation of objects using Gyroscopes and Accelerometers.
While an Accelerometer is usually used to measure gravity it can be used to measure the inclination of a body with respect to the surface of earth along the x and y axis(not z axis as Z axis faces the direction opposite the direction of gravitional force) by measuring the direction in which the force of gravity is felt.
Gyroscope measures the rate of rotation about one or all the axes of a body. While it gives fairly accurate estimation of the angular velocity, if we use it to calculate the current inclination based on the starting inclination and the angular velocity, there is a lot of drift, which means the gyroscope error will accumulate over time as we calculate newer angles based on previous angle and angular velocity and the error in angular velocity piles on leading to increasingly inaccurate estimations as time passes.
A real life example of how Kalman filter works is noticed while driving on a highway in a car. If you take the time passed since when your started driving and your estimated average speed since then and use it to calculate the distance you have traveled your calculation will become more inaccurate as time passes.
This is drift in value. However if you correct based on each milestone marker that you pass through and re-calculate your distance travelled using milestone data and your average speed since you pass the last milestone your result will be much more accurate irrespective of how much time has passed. That is approximately close to how Kalman filter and sensor fusion work.
State Sensor: 
Delta Sensor: 
Shantanu Bhadoria https://www.shantanubhadoria.com
type FilterData struct {
/*
State the state sensor value. In a IMU this would be the
Accelerometer
*/
State float64
/*
Bias: the delta sensor error. This is the deviation
from sensor reading and actual value. Bias can be caused by
electromagnetic interference and represents a permanent error
in delta sensor reading. Bias is detected by averaging the
delta sensor reading at stationary state of delta sensor
*/
Bias float64
/*
Covariance Matrix a 2d 2x2 matrix (also known as dispersion
matrix or variance-covariance matrix) is a matrix whose
element in the i, j position is the covariance between the i
and j elements of a random vector. Leave this at default
value of [[0,0],[0,0]]
*/
Covariance [2][2]float64
QAngle float64
QBias float64
RMeasure float64
}FilterData struct, initialize this struct before commencing any operations, as sensors are read, this struct must be updated alongside
func (filterData *FilterData) Update(stateReading, deltaReading, deltaTime float64) float64Call this method to update the state value based on sensor fusion of state and delta sensor and the previously calculated reading to get progressively more accurate state values