- About
- How to use
- Main algorithm and equations
- 1-D Kalman filter
- 2-D Kalman filter
- Bounding Box Kalman filter (KalmanBBox)
- Support
- Dependencies
- License
- Devs
- References
The Kalman filter estimates the state of a system at time
In other words, the purpose of Kalman filter is to predict the next state via using prior knowledge of the current state.
In this repository Hybrid Kalman filter is implemented considering continuous-time model while discrete-time measurements. See the ref. - https://en.wikipedia.org/wiki/Kalman_filter#Hybrid_Kalman_filter
You can find version for Rust programming language also - link
| Kalman 1D | Kalman 2D |
|---|---|
 |
 |
| Kalman BBox |
|---|
 |
A showcase how to visualize Kalman filter works
Simply add dependency into your project:
go get github.com/LdDl/kalman-filterStart using it, e.g. Kalman2D:
package main
import (
"encoding/csv"
"fmt"
"os"
kalman_filter "github.com/LdDl/kalman-filter"
)
func main() {
dt := 0.04 // 1/25 = 25 fps - just an example
ux := 1.0
uy := 1.0
stdDevA := 2.0
stdDevMx := 0.1
stdDevMy := 0.1
// Sample measurements
// Note: in this example Y-axis going from up to down
xs := []float64{311, 312, 313, 311, 311, 312, 312, 313, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 311, 311, 311, 311, 311, 310, 311, 311, 311, 310, 310, 308, 307, 308, 308, 308, 307, 307, 307, 308, 307, 307, 307, 307, 307, 308, 307, 309, 306, 307, 306, 307, 308, 306, 306, 306, 305, 307, 307, 307, 306, 306, 306, 307, 307, 308, 307, 307, 308, 307, 306, 308, 309, 309, 309, 309, 308, 309, 309, 309, 308, 311, 311, 307, 311, 307, 313, 311, 307, 311, 311, 306, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312}
ys := []float64{5, 6, 8, 10, 11, 12, 12, 13, 16, 16, 18, 18, 19, 19, 20, 20, 22, 22, 23, 23, 24, 24, 28, 30, 32, 35, 39, 42, 44, 46, 56, 58, 70, 60, 52, 64, 51, 70, 70, 70, 66, 83, 80, 85, 80, 98, 79, 98, 61, 94, 101, 94, 104, 94, 107, 112, 108, 108, 109, 109, 121, 108, 108, 120, 122, 122, 128, 130, 122, 140, 122, 122, 140, 122, 134, 141, 136, 136, 154, 155, 155, 150, 161, 162, 169, 171, 181, 175, 175, 163, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178}
// Assume that initial X,Y coordinates match the first measurement
ix := xs[0] // Initial state for X
iy := ys[0] // Initial state for Y
kalman := kalman_filter.NewKalman2D(dt, ux, uy, stdDevA, stdDevMx, stdDevMy, kalman_filter.WithState2D(ix, iy))
predictions := make([][]float64, 0, len(xs))
updatedStates := make([][]float64, 0, len(xs))
for i := 0; i < len(xs); i++ {
// Considering that the measurements are noisy
mx := xs[i]
my := ys[i]
// Predict stage
kalman.Predict()
state := kalman.GetVectorState()
predictions = append(predictions, []float64{state.At(0, 0), state.At(1, 0)})
// Update stage
err := kalman.Update(mx, my)
if err != nil {
fmt.Println(err)
return
}
updatedState := kalman.GetVectorState()
updatedStates = append(updatedStates, []float64{updatedState.At(0, 0), updatedState.At(1, 0)})
}
file, err := os.Create("kalman-2d.csv")
if err != nil {
fmt.Println(err)
return
}
defer file.Close()
writer := csv.NewWriter(file)
defer writer.Flush()
writer.Comma = ';'
err = writer.Write([]string{"measurement X", "measurement Y", "prediction X", "prediction Y", "updated X", "updated Y"})
if err != nil {
fmt.Println(err)
return
}
for i := 0; i < len(xs); i++ {
err = writer.Write([]string{
fmt.Sprintf("%f", xs[i]),
fmt.Sprintf("%f", ys[i]),
fmt.Sprintf("%f", predictions[i][0]),
fmt.Sprintf("%f", predictions[i][1]),
fmt.Sprintf("%f", updatedStates[i][0]),
fmt.Sprintf("%f", updatedStates[i][1]),
})
if err != nil {
fmt.Println(err)
return
}
}
}Define mentioned linear stochastic difference equation:
Define measurement model:
Let's denote variables:
-
$A$ (sometimes it's written as$F$ , but I prefer to stick with$A$ ) - Transition matrix of size$n \times n$ relating state$k-1$ to state$k$ -
$B$ - Control input matrix of size$n \times l$ which is applied to optional control input$u_{k-1}$ -
$H$ - Transformation (observation) matrix of size$m \times n$ . -
$u_{k}$ - Control input -
$w_{k}$ - Process noise vector with covariance$Q$ . Gaussian noise with the normal probability distribution:$$w(t) \sim N(0, Q) \tag{3}$$ -
$v_{k}$ - Measurement noise vector (uncertainty) with covariance$R$ . Gaussian noise with the normal probability distribution:$$v(t) \sim N(0, R) \tag{4}$$
Let's use the dash sign "
A priory state in matrix notation is defined as
Note: A posteriory state
Error covariance matrix
Note:
The Kalman gain (which minimizes the estimate variance) in matrix notation is defined as:
After evaluating the Kalman gain we need to update a priory state
Then we can update predicted state
After that we should update error covariance matrix
The whole algorithm can be described as high-level diagram:
Fig 1. Operation of the Kalman filter. Welch & Bishop, 'An Introduction to the Kalman Filter'
Considering acceleration motion let's write down its equations:
Velocity:
Position:
Let's write
State vector
Matrix form of
Taking close look on
Let's find transformation matrix
Notice:
Process noise covariance matrix
Since we know about
And now process noise covariance matrix
Covariance of measurement noise
Golang implementation is here
Example of usage:
rand.Seed(1337)
dt := 0.1
u := 2.0
stdDevA := 0.25
stdDevM := 1.2
n := 100
iters := int(float64(n) / dt)
track := make([]struct {
t float64
x float64
}, iters)
v := 0.0
for i := 0; i < iters; i++ {
track[i] = struct {
t float64
x float64
}{
t: v,
x: dt * (v*v - v),
}
v += dt
}
kalman := kalman_filter.NewKalman1D(dt, u, stdDevA, stdDevM)
measurements := make([]float64, 0, iters)
predictions := make([]float64, 0, iters)
for _, val := range track {
// tm := val.t
x := val.x
// Add some noise to perfect track
noise := rand.Float64()*100 - 50
z := kalman.H.At(0, 0)*x + noise
measurements = append(measurements, z)
// Predict stage
kalman.Predict()
state := kalman.GetVectorState()
predictions = append(predictions, state.At(0, 0))
// Update stage
err := kalman.Update(z)
if err != nil {
fmt.Println(err)
return
}
}
fmt.Println("time;perfect;measurement;prediction")
for i := 0; i < len(track); i++ {
fmt.Printf("%f;%f;%f;%f\n", track[i].t, track[i].x, measurements[i], predictions[i])
}How exported chart does look like:
Considering acceleration motion again let's write down its equations:
Considering the same physical model as in
Matrix form of
Taking close look on
Let's find transformation matrix
Process noise covariance matrix
Since we know about
And now process noise covariance matrix
Covariance of measurement noise
Golang implementation is here
Example of usage:
dt := 0.04 // 1/25 = 25 fps - just an example
ux := 1.0
uy := 1.0
stdDevA := 2.0
stdDevMx := 0.1
stdDevMy := 0.1
// Sample measurements
// Note: in this example Y-axis going from up to down
xs := []float64{311, 312, 313, 311, 311, 312, 312, 313, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 311, 311, 311, 311, 311, 310, 311, 311, 311, 310, 310, 308, 307, 308, 308, 308, 307, 307, 307, 308, 307, 307, 307, 307, 307, 308, 307, 309, 306, 307, 306, 307, 308, 306, 306, 306, 305, 307, 307, 307, 306, 306, 306, 307, 307, 308, 307, 307, 308, 307, 306, 308, 309, 309, 309, 309, 308, 309, 309, 309, 308, 311, 311, 307, 311, 307, 313, 311, 307, 311, 311, 306, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312}
ys := []float64{5, 6, 8, 10, 11, 12, 12, 13, 16, 16, 18, 18, 19, 19, 20, 20, 22, 22, 23, 23, 24, 24, 28, 30, 32, 35, 39, 42, 44, 46, 56, 58, 70, 60, 52, 64, 51, 70, 70, 70, 66, 83, 80, 85, 80, 98, 79, 98, 61, 94, 101, 94, 104, 94, 107, 112, 108, 108, 109, 109, 121, 108, 108, 120, 122, 122, 128, 130, 122, 140, 122, 122, 140, 122, 134, 141, 136, 136, 154, 155, 155, 150, 161, 162, 169, 171, 181, 175, 175, 163, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178}
// Assume that initial X,Y coordinates match the first measurement
ix := xs[0] // Initial state for X
iy := ys[0] // Initial state for Y
kalman := kalman_filter.NewKalman2D(dt, ux, uy, stdDevA, stdDevMx, stdDevMy, kalman_filter.WithState2D(ix, iy))
predictions := make([][]float64, 0, len(xs))
updatedStates := make([][]float64, 0, len(xs))
for i := 0; i < len(xs); i++ {
// Considering that the measurements are noisy
mx := xs[i]
my := ys[i]
// Predict stage
kalman.Predict()
state := kalman.GetVectorState()
predictions = append(predictions, []float64{state.At(0, 0), state.At(1, 0)})
// Update stage
err := kalman.Update(mx, my)
if err != nil {
fmt.Println(err)
return
}
updatedState := kalman.GetVectorState()
updatedStates = append(updatedStates, []float64{updatedState.At(0, 0), updatedState.At(1, 0)})
}
fmt.Println("measurement X;measurement Y;prediction X;prediction Y;updated X;updated Y")
for i := 0; i < len(xs); i++ {
fmt.Printf("%f;%f;%f;%f;%f;%f\n", xs[i], ys[i], predictions[i][0], predictions[i][1], updatedStates[i][0], updatedStates[i][1])
}How exported chart does look like:
For multi-object tracking (MOT) purposes, tracking only center position
Considering constant velocity model for both position and size, let's write down the equations.
For center position (same physical model as in
For bounding box dimensions (analogous dynamics):
And for velocities:
State vector
Matrix form of
Taking close look on
Let's find transformation matrix
Process noise covariance matrix
Since we know about
Assuming uniform acceleration noise
Covariance of measurement noise
For multi-object tracking, we need to associate detections with existing tracks. The Mahalanobis distance accounts for prediction uncertainty unlike simple Euclidean distance.
The innovation (measurement residual) is defined as:
The innovation covariance matrix
The squared Mahalanobis distance between measurement
The Mahalanobis distance:
The squared Mahalanobis distance follows a chi-squared distribution with
Gating threshold
Common thresholds for
| Degrees of freedom |
Threshold |
|---|---|
| 2 (position only) | 5.991 |
| 4 (bounding box) | 9.488 |
A detection passes the gate (can be associated with track) if:
Golang implementation is here
Example of usage:
dt := 0.04 // 1/25 = 25 fps - just an example
uCx := 1.0
uCy := 1.0
uW := 0.0 // Usually no control input for size
uH := 0.0
stdDevA := 2.0
stdDevMCx := 0.1
stdDevMCy := 0.1
stdDevMW := 0.1
stdDevMH := 0.1
// Sample measurements (center_x, center_y, width, height)
measurements := []struct{ cx, cy, w, h float64 }{
{100.0, 50.0, 40.0, 80.0},
{102.0, 52.0, 41.0, 81.0},
{105.0, 55.0, 42.0, 82.0},
// ... more measurements
}
// Assume that initial state matches the first measurement
iCx := measurements[0].cx
iCy := measurements[0].cy
iW := measurements[0].w
iH := measurements[0].h
kalman := kalman_filter.NewKalmanBBox(
dt, uCx, uCy, uW, uH,
stdDevA,
stdDevMCx, stdDevMCy, stdDevMW, stdDevMH,
kalman_filter.WithStateBBox(iCx, iCy, iW, iH),
)
predictions := make([][4]float64, 0, len(measurements))
updatedStates := make([][4]float64, 0, len(measurements))
for _, m := range measurements {
// Predict stage
kalman.Predict()
pCx, pCy, pW, pH := kalman.GetState()
predictions = append(predictions, [4]float64{pCx, pCy, pW, pH})
// Update stage
err := kalman.Update(m.cx, m.cy, m.w, m.h)
if err != nil {
fmt.Println(err)
return
}
uCx, uCy, uW, uH := kalman.GetState()
updatedStates = append(updatedStates, [4]float64{uCx, uCy, uW, uH})
}
fmt.Println("measurement cx;measurement cy;measurement w;measurement h;prediction cx;prediction cy;prediction w;prediction h")
for i, m := range measurements {
p := predictions[i]
fmt.Printf("%f;%f;%f;%f;%f;%f;%f;%f\n", m.cx, m.cy, m.w, m.h, p[0], p[1], p[2], p[3])
}Example with Mahalanobis distance for data association:
// After prediction, check if a new detection can be associated
newDetection := struct{ cx, cy, w, h float64 }{108.0, 58.0, 43.0, 83.0}
// Get squared Mahalanobis distance
mahalDistSq, err := kalman.MahalanobisDistanceSquared(
newDetection.cx, newDetection.cy, newDetection.w, newDetection.h,
)
if err != nil {
fmt.Println(err)
return
}
// Gating threshold for 4 DOF at 95% confidence
threshold := 9.488
if mahalDistSq < threshold {
// Detection passes the gate - can be associated with this track
kalman.Update(newDetection.cx, newDetection.cy, newDetection.w, newDetection.h)
} else {
// Detection is too far - likely belongs to different object
}How exported chart does look like:
If you have troubles or questions please open an issue.
PR's are welcome.
- Matrix computations - gonum. License is BSD 3-Clause "New" or "Revised" License. Link
- Errors wraping - errors. License is BSD 2-Clause "Simplified" License. Link
License of this library is MIT.
You can check it here
Pavel7824 https://github.com/Pavel7824
- Greg Welch and Gary Bishop, ‘An Introduction to the Kalman Filter’, July 24, 2006
- Introducion to the Kalman Filter by Alex Becker
- Kalman filter on wikipedia
- State-transition matrix
- Python implementation by Rahmad Sadli
I did struggle on displaying matrices in GitHub's MathJax markdown. If you know better way to do it you are welcome