Kalman Filter

An algorithm uses a series of smoothing noisy measurements over time

In One Line

Too Noise? Filter it. Make Noise? Take me. Burst? Ready.

Package Info

display name

AceLand Kalman Filter

package name

com.aceland.kalmanfilter

latest version

2.2.1

namespace

AceLand.KalmanFilter

git repository

https://github.com/parsue/com.aceland.kalmanfilter.git

dependencies

com.unity.mathematics: 1.3.2
com.unity.burst: 1.8.25
com.aceland.library: 2.0.10

Overview

Kalman Filter is known as linear quadratic estimation. Simple put, this is a denoise filter.

please read this wiki document for details.

In wiki document, it may be too complicated if you are not familiar to Mathmatics. Basically we use the following values to estimate the result (prediction). What you need is to know the following values:

process noise covariance. How much we expect the true value to change between measurements. (model uncertainty) default: 1e-6f // 0.000001

measurement noise covariance. How noisy the sensor/measurement is. default: 1e-3f // 0.001

estimate uncertainty after the measurement update step. (a scalar variance-like quantity) default: 1

kalman gain (0..1) the weight given to the new measurement relative to the current estimate.

current state estimate. (the smoothed value)

Summary

A compact, scalar Kalman filter suitable for smoothing noisy readings.
Tune Q and R to balance smoothness vs responsiveness.
Use Update for streaming data or batch updates for historical sequences.
Inspect and control state via GetCurrentValues, SetValues, and Reset.
Customizable value adapter can modularizecalculation process.
Jobs and ECS ready.

Machanizm

Core idea in plain words

The filter maintains a running best-guess X of the true value and a notion of confidence P.
Each new measurement is blended with the current estimate using a weight K (the Kalman gain).
If measurements are noisy (high R), the filter trusts its prior estimate more; if the process is volatile (high Q), it adapts faster to change.
Over time, the filter converges to a smooth, lag-minimized estimate.

Update math (intuitive)

Optionally update noise parameters:
- If newQ or newR are provided, the filter updates Q and/or R for this call onward.
Compute the Kalman gain K using current uncertainty and noise:
- Predict uncertainty: P + Q.
- Combine with measurement noise: P + Q + R.
- Gain: K = (P + Q) / (P + Q + R).
- Intuition: If R is large (noisy sensor), K gets smaller; if Q is large (we expect changes), K gets larger.
Update the posterior uncertainty P:
- P = R * (P + Q) / (R + P + Q).
- Intuition: After incorporating the measurement, our uncertainty shrinks, bounded by measurement noise R.
Update the state estimate X:
- X_new = X_old + K * (measurement − X_old).
- A weighted blend: K close to 1 tracks the measurement closely; K near 0 barely moves.

Choosing Q and R

R (measurement noise):
- Larger R if your sensor is noisy; this makes the filter smoother and slower to react.
- Smaller R if measurements are reliable; the filter tracks input more tightly.
Q (process noise):
- Larger Q if the underlying true value changes rapidly; the filter adapts faster to new trends.
- Smaller Q if the true value is stable; the filter will be steadier.

Quick Start

build a Kalman Filter of supported type.

IKalmanFilter<float> myKalmanFilter;

// build with default values
myKalmanFilter = KalmanFilter.Builder<float>();

// build with values
myKalmanFilter = KalmanFilter.Builder<float>(
    q: 1e-5f,
    r: 1e-3f,
    p: 1f
);

By updating new value in every time state, kalman filter will calculate the current state estimated value.

The filter will return the current value on Update.

IKalmanFilter<float> myKalmanFilter;

private void Update()
{
    // update value of current state
    float currentValue = myKalmanFilter.Update(newValue);
    
    // update value with new q and r
    float currentValue = myKalmanFilter.Update(newValue, newQ: q, newR: r);
}

In case of not updating values in several time states, update batched values is supported in our Kalman Filter.

IKalmanFilter<float> myKalmanFilter;

List<float> missedValues = new List<float>()
{
    12.3f, 84.6f, -24.762f, 7.34f
};

float currentValue = myKalmanFilter.Update(amissedValues);
float currentValue = myKalmanFilter.Update(amissedValues, newQ: q, newR: r);

Values x, p and k can be get from or set to the filter. This helps transfering the experience to another filter.

IKalmanFilter<float> oldFilter;
IKalmanFilter<float> newFilter;

(float x, float p, float k) kf = oldFilter.GetCurrentValues();
newFilter.SetValues(kf.x, kf.p, kf.k);

Supported Types

Our simplified Kalman Filter supported following types in Unity:

float
Vector2
Vector3
Vector4

Value Adapter

Providing a customable type calcuation to Kalman Filter for supporting any unmanaged types.

Data must be a struct, and contains with unmanaged types.

class can be worked in the struct, but not supported for Burst.

public struct MyPose
{
    public Vector3 Position;
    public Quaternion Rotation;
}

Adapter contains simple calculate method for Kalman Filter to operate.

In this example, the prediction result will modify to vlaues in MyPose.

public sealed class MyPoseAdapter : IKalmanValueAdapter<MyPose>
{
    public MyPose Zero => default;
    public MyPose Add(MyPose l, MyPose r) => new MyPose
    {
        Position = l.Position + r.Position,
        Rotation = Quaternion.SlerpUnclamped(l.Rotation, r.Rotation, 1f)
    };
    public MyPose Subtract(MyPose l, MyPose r) => new MyPose
    {
        Position = l.Position - r.Position,
        Rotation = Quaternion.Inverse(r.Rotation) * l.Rotation
    };
    public MyPose Scale(MyPose v, float s) => new MyPose
    {
        Position = v.Position * s,
        Rotation = Quaternion.Slerp(Quaternion.identity, v.Rotation, s)
    };
}

If the adapter is pending to apply in serveral Kalman Filters, please register the adapter.

var myPoseAdapter = new MyPoseAdapter();
KalmanFilter.RegisterAdapter<MyPose>(myPoseAdapter);

var poseFilterA = KalmanFilter<MyPose>.Build();
var poseFilterB = KalmanFilter<MyPose>.Build();

If the adapter is applying in single Kalman Filters only, set the adapter when building.

var myPoseAdapter = new MyPoseAdapter();
var poseFilter = KalmanFilter<MyPose>.Build(
    adapter: myPoseAdapter
);

Jobs and ECS

It's easy to create a Burst Kalman Filter and Value Adapter for Jobs or ECS system.

In next version, we will create default value adapters for Jobs and ECS in default, and simplify the use of Burst Kalman Filter.

For burst compile, an adapter MUST be readonly struct .

[BurstCompile]
public readonly struct FloatAdapter : IKalmanValueAdapter<float>
{
    public float Zero => 0f;
    public float Add(float left, float right) => left + right;
    public float Subtract(float left, float right) => left - right;
    public float Scale(float value, float scalar) => value * scalar;
}

Guidelines

Mark them readonly struct.
Do not capture managed state (no reference-type fields, no string, no delegates).
Only use Burst-supported APIs (Unity.Mathematics.math where helpful).
Expose a static instance if you need to reuse the adapter without allocations:
```
public static readonly FloatAdapter Instance = default;
```

Adding a field in Job with concrete BurstKalmanFilter<T> instead of interface.

[BurstCompile]
public struct FilterJob : IJobParallelFor
{
    [ReadOnly] public NativeArray<float> Measurements;
    public BurstKalmanFilter<float, FloatAdapter> Filter;

    public void Execute(int index)
    {
        Filtered[index] = Filter.Update(Measurements[index]);
    }
}

Creating a BurstKalmanFilter with adapter, and inject to the job system.

var filter = BurstKalmanFilter<float, FloatAdapter>.Build(
    ops: FloatAdapter.Instance
);
var job = new FilterJob
{
    Measurements = measurements,
    Filter = filter
};
job.Schedule(Measurements.Length, 64).Complete();

Best Practice

Practical guidance

Start with small Q (e.g., 1e-6 to 1e-3) and moderate R (e.g., 1e-3 to 1e-1). Tune empirically.
If you see lag (filter responds too slowly), increase Q or decrease R slightly.
If you see jitter (filter chases noise), decrease Q or increase R.

State management

GetCurrentValues lets you inspect the internal state for debugging or telemetry.
SetValues lets you initialize with a known starting estimate:
- For example, set X to your first measurement, P to a moderate value (e.g., 1), K to 0.
Reset sets the filter to a neutral starting point (P=1, X=default, K=0).

FAQ

Why is K between 0 and 1?

Because K is a ratio of variances; it represents a blending weight between your prior estimate and the measurement.

Do I need to set P manually?

Not usually. P starts at 1 after Reset and then adapts. If you have a strong prior, set P smaller to indicate higher confidence.

What if measurements come at uneven intervals?

This scalar filter assumes a fixed implicit timestep. For highly irregular intervals, Q should ideally scale with time; that logic isn’t included here.

Can I change Q and R on every update?

Yes. Use newQ/newR parameters to adapt on-the-fly (e.g., dynamic sensor noise estimation).

PreviousChange Log NextChange Log

Last updated 1 day ago