{"id":21235965,"url":"https://github.com/lddl/kalman-filter","last_synced_at":"2025-10-04T14:28:35.124Z","repository":{"id":57574576,"uuid":"353795445","full_name":"LdDl/kalman-filter","owner":"LdDl","description":"Dead simple Linear Kalman Filter. Contains 2-D based tracker","archived":false,"fork":false,"pushed_at":"2023-06-24T14:50:06.000Z","size":218,"stargazers_count":11,"open_issues_count":0,"forks_count":1,"subscribers_count":3,"default_branch":"master","last_synced_at":"2024-07-20T15:08:38.076Z","etag":null,"topics":["filter","kalman","kalman-filter","kalman-tracker","kalman-tracking","object-tracking","point-tracking","prediction","tracker","tracking"],"latest_commit_sha":null,"homepage":"","language":"Go","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"mit","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/LdDl.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":"LICENSE.md","code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null}},"created_at":"2021-04-01T18:45:22.000Z","updated_at":"2024-06-10T11:39:54.000Z","dependencies_parsed_at":"2024-01-06T05:34:12.290Z","dependency_job_id":"403d5bad-f21f-4f19-8238-a6b58bf9a4d7","html_url":"https://github.com/LdDl/kalman-filter","commit_stats":{"total_commits":25,"total_committers":2,"mean_commits":12.5,"dds":"0.16000000000000003","last_synced_commit":"21541044be77c6a9e83c7e16719c49b7c8749348"},"previous_names":[],"tags_count":4,"template":false,"template_full_name":null,"purl":"pkg:github/LdDl/kalman-filter","repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/LdDl%2Fkalman-filter","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/LdDl%2Fkalman-filter/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/LdDl%2Fkalman-filter/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/LdDl%2Fkalman-filter/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/LdDl","download_url":"https://codeload.github.com/LdDl/kalman-filter/tar.gz/refs/heads/master","sbom_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/LdDl%2Fkalman-filter/sbom","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":264619108,"owners_count":23638407,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["filter","kalman","kalman-filter","kalman-tracker","kalman-tracking","object-tracking","point-tracking","prediction","tracker","tracking"],"created_at":"2024-11-21T00:05:13.010Z","updated_at":"2025-10-04T14:28:30.062Z","avatar_url":"https://github.com/LdDl.png","language":"Go","funding_links":[],"categories":[],"sub_categories":[],"readme":"## Implementation of Discrete Kalman filter for object tracking purposes\n[![GoDoc](https://godoc.org/github.com/LdDl/kalman-filter?status.svg)](https://godoc.org/github.com/LdDl/kalman-filter) [![Sourcegraph](https://sourcegraph.com/github.com/LdDl/kalman-filter/-/badge.svg)](https://sourcegraph.com/github.com/LdDl/kalman-filter?badge) [![Go Report Card](https://goreportcard.com/badge/github.com/LdDl/kalman-filter)](https://goreportcard.com/report/github.com/LdDl/kalman-filter) [![GitHub tag](https://img.shields.io/github/tag/LdDl/kalman-filter.svg)](https://github.com/LdDl/kalman-filter/releases)[![Build Status](https://travis-ci.com/LdDl/kalman-filter.svg?branch=master)](https://travis-ci.com/LdDl/kalman-filter)\n\n## Table of Contents\n\n- [About](#about)\n- [How to use](#how-to-use)\n- [Main algorithm and equations](#main-algorithm-and-equations)\n- [Support](#support-and-contribution)\n- [Dependencies](#dependencies)\n- [License](#license)\n- [Devs](#developers)\n- [References](#References)\n\n## About\n\nThe Kalman filter estimates the state of a system at time $k$ via the linear stochastic difference equation considering the state of a system at time $k$ is evolved from the previous state at time $k-1$. See the ref. https://en.wikipedia.org/wiki/Kalman_filter\n\nIn other words, the purpose of Kalman filter is to predict the next state via using prior knowledge of the current state. \n\nIn 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\n\n__You can find version for Rust programming language also - [link](https://github.com/LdDl/kalman-rs)__\n\nKalman 1D            |  Kalman 2D\n:-------------------------:|:-------------------------:\n\u003cimg src=\"data/kalman-1d.png\" width=\"640\"\u003e  |  \u003cimg src=\"data/kalman-2d.png\" width=\"640\"\u003e\n\n\u003cp style=\"text-align: center;\"\u003e\u003ci\u003eA showcase how to visualize Kalman filter works\u003c/i\u003e\u003c/p\u003e\n\n## How to use\n\nSimply add dependency into your project:\n```shell\ngo get github.com/LdDl/kalman-filter\n```\n\nStart using it, e.g. Kalman2D:\n```go\npackage main\n\nimport (\n\t\"encoding/csv\"\n\t\"fmt\"\n\t\"os\"\n\n\tkalman_filter \"github.com/LdDl/kalman-filter\"\n)\n\nfunc main() {\n\tdt := 0.04 // 1/25 = 25 fps - just an example\n\tux := 1.0\n\tuy := 1.0\n\tstdDevA := 2.0\n\tstdDevMx := 0.1\n\tstdDevMy := 0.1\n\n\t// Sample measurements\n\t// Note: in this example Y-axis going from up to down\n\txs := []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}\n\tys := []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}\n\n\t// Assume that initial X,Y coordinates match the first measurement\n\tix := xs[0] // Initial state for X\n\tiy := ys[0] // Initial state for Y\n\n\tkalman := kalman_filter.NewKalman2D(dt, ux, uy, stdDevA, stdDevMx, stdDevMy, kalman_filter.WithState2D(ix, iy))\n\n\tpredictions := make([][]float64, 0, len(xs))\n\tupdatedStates := make([][]float64, 0, len(xs))\n\tfor i := 0; i \u003c len(xs); i++ {\n\t\t// Considering that the measurements are noisy\n\t\tmx := xs[i]\n\t\tmy := ys[i]\n\n\t\t// Predict stage\n\t\tkalman.Predict()\n\t\tstate := kalman.GetVectorState()\n\t\tpredictions = append(predictions, []float64{state.At(0, 0), state.At(1, 0)})\n\n\t\t// Update stage\n\t\terr := kalman.Update(mx, my)\n\t\tif err != nil {\n\t\t\tfmt.Println(err)\n\t\t\treturn\n\t\t}\n\t\tupdatedState := kalman.GetVectorState()\n\t\tupdatedStates = append(updatedStates, []float64{updatedState.At(0, 0), updatedState.At(1, 0)})\n\t}\n\n\tfile, err := os.Create(\"kalman-2d.csv\")\n\tif err != nil {\n\t\tfmt.Println(err)\n\t\treturn\n\t}\n\tdefer file.Close()\n\n\twriter := csv.NewWriter(file)\n\tdefer writer.Flush()\n\twriter.Comma = ';'\n\n\terr = writer.Write([]string{\"measurement X\", \"measurement Y\", \"prediction X\", \"prediction Y\", \"updated X\", \"updated Y\"})\n\tif err != nil {\n\t\tfmt.Println(err)\n\t\treturn\n\t}\n\tfor i := 0; i \u003c len(xs); i++ {\n\t\terr = writer.Write([]string{\n\t\t\tfmt.Sprintf(\"%f\", xs[i]),\n\t\t\tfmt.Sprintf(\"%f\", ys[i]),\n\t\t\tfmt.Sprintf(\"%f\", predictions[i][0]),\n\t\t\tfmt.Sprintf(\"%f\", predictions[i][1]),\n\t\t\tfmt.Sprintf(\"%f\", updatedStates[i][0]),\n\t\t\tfmt.Sprintf(\"%f\", updatedStates[i][1]),\n\t\t})\n\t\tif err != nil {\n\t\t\tfmt.Println(err)\n\t\t\treturn\n\t\t}\n\t}\n}\n```\n\n## Main algorithm and equations\n\nDefine mentioned _linear stochastic difference equation_:\n\n$$\\chi_{k} = A⋅\\chi_{k-1} + B⋅u_{k-1} + w_{k-1} \\tag{1}$$\n\nDefine measurement model:\n$$z_{k} = H⋅\\chi_{k} + v_{k}\\tag{2}$$\n\nLet's denote variables:\n\n* $A$ (sometimes it's written as $F$, but I prefer to stick with $A$) - [Transition matrix](https://en.wikipedia.org/wiki/State-transition_matrix) of size $n \\times n$ relating state $k-1$ to state $k$\n* $B$ - Control input matrix of size $n \\times l$ which is applied to *optional* control input $u_{k-1}$\n* $H$ - Transformation (observation) matrix of size $m \\times n$.\n* $u_{k}$ - Control input\n* $w_{k}$ - Process noise vector with covariance $Q$. Gaussian noise with the normal probability distribution:\n$$w(t) \\sim N(0, Q) \\tag{3}$$\n* $v_{k}$ - Measurement noise vector (uncertainty) with covariance $R$. Gaussian noise with the normal probability distribution:\n$$v(t) \\sim N(0, R) \\tag{4}$$\n\n### Prediction\n\nLet's use the dash sign \" $-$ \" as superscript to indicate the a priory state.\n\nA priory state in matrix notation is defined as\n\n$$\\hat{\\chi}^-_ {k} = A⋅\\hat{\\chi}_ {k-1} + B⋅u_ {k-1} \\tag{5}$$\n\n$$\\text{, where $\\hat{\\chi}^-_ {k}$ - a priory state (a.k.a. predicted),  $\\hat{\\chi}_ {k-1}$ - a posteriory state (a.k.a. previous)} $$\n\n__Note: A posteriory state $\\hat{\\chi}_{k-1}$ on 0-th time step (initial) should be *guessed*__\n\nError covariance matrix $P^-$ is defined as\n\n$$P^-_ {k} =  A⋅P_ {k-1}⋅A^{T} + Q \\tag{6}$$\n\n$$\\text{, where $P_ {k-1}$ - previously estimated error covariance matrix of size $n \\times n$ (should match transition matrix dimensions), } $$\n$$\\text{Q - process noise covariance}$$\n\n__Note:__ $P_ {k-1}$ __on 0-th time step (initial) should be *guessed*__\n\n### Correction\n\nThe Kalman gain (which minimizes the estimate variance) in matrix notation is defined as:\n\n$$K_ {k} = P^-_ {k}⋅H^{T}⋅(H⋅P^-_ {k}⋅H^{T}+R)^{-1} \\tag{7}$$\n\n$$\\text{, where H - transformation matrix, R - measurement noise covariance}$$\n\nAfter evaluating the Kalman gain we need to update a priory state $\\hat{\\chi}^-_ {k}$. In order to do that we need to calculate measurement residual:\n\n$$r_ {k} = z_ {k} - H⋅\\hat{\\chi}^-_ {k} \\tag{8}$$\n\n$$\\text{, where $z_ {k}$ - true measurement, $H⋅\\hat{\\chi}^-_ {k}$ - previously estimated measurement}$$\n\nThen we can update predicted state $\\hat{\\chi}_ {k}$:\n\n$$\\hat{\\chi}_ {k} = \\hat{\\chi}^-_ {k} + K_{k}⋅r_{k}$$\n\n$$\\text{or} \\tag{9}$$\n\n$$\\hat{\\chi}_ {k} = \\hat{\\chi}^-_ {k} + K_{k}⋅(z_{k} - H⋅\\hat{\\chi}^-_{k})$$\n\nAfter that we should update error covariance matrix $P_{k}$ which will be used in next time stap (an so on):\n$$P_{k} = (I - K_{k}⋅H)⋅P^-_{k}\\tag{10}$$\n$$\\text{, where $I$ - identity matrix (square matrix with ones on the main diagonal and zeros elsewhere)}$$\n\n\n### Overall\nThe whole algorithm can be described as high-level diagram:\n\u003cp align=\"center\"\u003e\n\u003cimg src=\"data/diagram.png\" width=\"720\" \u003e\n\u003cp align=\"center\"\u003eFig 1. Operation of the Kalman filter. Welch \u0026 Bishop, 'An Introduction to the Kalman Filter'\u003c/p\u003e\n\u003c/p\u003e\n\n## 1-D Kalman filter\n\nConsidering acceleration motion let's write down its equations:\n\nVelocity:\n$$v = v_{0} + at \\tag{11}$$\n$$v(t) = x'(t) $$\n$$a(t) = v'(t) = x''(t)$$\n\nPosition:\n$$x = x_{0} + v_{0}t + \\frac{at^2}{2} \\tag{12}$$\n\nLet's write $(11)$ and $(12)$ in Lagrange form:\n\n$$x'_ {k} = x'_ {k-1} + x''_{k-1}\\Delta t \\tag{13}$$\n\n$$x_{k} = x_{k-1} + x'_ {k-1}\\Delta t + \\frac{x''_{k-1}(\\Delta t^2)}{2} \\tag{14}$$\n\nState vector $\\chi_{k}$ looks like:\n\n$$\\chi_{k} = \\begin{bmatrix}\nx_{k} \\\\\nx'_ {k}\n\\end{bmatrix} = \\begin{bmatrix}\nx_{k-1} + x'_ {k-1}\\Delta t + \\frac{x''_ {k-1}(\\Delta t^2)}{2} \\\\\nx'_ {k-1} + x''_{k-1}\\Delta t\n\\end{bmatrix} \\tag{15}$$\n\nMatrix form of $\\chi_{k}$ :\n\n$$\\chi_{k} = \\begin{bmatrix} x_{k} \\\\\nx'_ {k} \\end{bmatrix} = \\begin{bmatrix} 1 \u0026 \\Delta t \\\\\n0 \u0026 1\\end{bmatrix} ⋅ \\begin{bmatrix} x_{k-1} \\\\\nx'_ {k-1} \\end{bmatrix} + \\begin{bmatrix} \\frac{\\Delta t^2}{2} \\\\\n\\Delta t \\end{bmatrix} ⋅ x''_ {k-1} = \\begin{bmatrix} 1 \u0026 \\Delta t \\\\\n0 \u0026 1\\end{bmatrix} ⋅ \\chi_{k-1} + \\begin{bmatrix} \\frac{\\Delta t^2}{2} \\\\\n\\Delta t \\end{bmatrix} ⋅ x''_{k-1} \\tag{16}$$\n\n\nTaking close look on $(16)$ and $(1)$ we can write transition matrix $A$ and control input matrix $B$ as follows:\n\n$$A = \\begin{bmatrix} 1 \u0026 \\Delta t \\\\\n0 \u0026 1\\end{bmatrix} \\tag{17}$$\n\n$$B = \\begin{bmatrix} \\frac{\\Delta t^2}{2} \\\\\n\\Delta t \\end{bmatrix} \\tag{18}$$\n\nLet's find transformation matrix $H$. According to $(2)$:\n\n$$z_{k} = H⋅\\chi_{k} + v_{k} = \\begin{bmatrix} 1 \u0026 0 \\end{bmatrix} ⋅\\begin{bmatrix} x_{k} \\\\\n{x'_ {k}} \\end{bmatrix} + v_{k} \\tag{19}$$\n\n$$ H = \\begin{bmatrix} 1 \u0026 0 \\end{bmatrix} \\tag{20}$$\n\n__Notice:__ $v_{k}$ __in__ $(19)$ __- is not speed, but measurement noise! Don't be confused with notation. E.g.:__ \n\n$$ \\text{$ \\chi_{k} = \\begin{bmatrix} 375.74 \\\\\n0 - \\text{assume zero velocity}  \\end{bmatrix} $, $ v_{k} = 2.64 =\u003e $} $$\n\n$$ \\text{$ =\u003e z_{k} = \\begin{bmatrix} 1 \u0026 0 \\end{bmatrix} ⋅\\begin{bmatrix} 375.74 \\\\\n0 \\end{bmatrix} + 2.64 = \\begin{bmatrix} 378.38 \u0026 2.64 \\end{bmatrix} $ - you can see that first vector argument it is just noise $v_{k}$ added}$$\n\n$$ \\text{to observation $x_{k}$ and the second argument is noise $v_{k}$ itself.}$$\n\nProcess noise covariance matrix $Q$:\n\n$$Q = \\begin{matrix}\n \u0026 \\begin{matrix}x \u0026\u0026 x'\\end{matrix} \\\\\n\\begin{matrix}x \\\\\nx'\\end{matrix} \u0026 \n  \\begin{bmatrix} \\sigma^2_{x} \u0026 \\sigma_{x} \\sigma_{x'} \\\\\n\\sigma_{x'} \\sigma_{x} \u0026 \\sigma^2_{x'}\\end{bmatrix}\n \\\\\\\\\n\\end{matrix} \\tag{21}$$\n\n$$\\text{, where} $$\n\n$$ \\text{$\\sigma_{x}$ - standart deviation of position} $$\n\n$$ \\text{$\\sigma_{x'}$ - standart deviation of velocity} $$\n\nSince we know about $(14)$ we can define $\\sigma_{x}$ and $\\sigma_{x'}$ as:\n\n$$ \\sigma_{x} = \\sigma_{x''} \\frac{\\Delta t^2}{2} \\tag{22}$$\n\n$$ \\sigma_{x'} = \\sigma_{x''} \\Delta t \\tag{23}$$\n\n$$\\text{, where $\\sigma_{x''}$ - standart deviation of acceleration (tuned value)} $$\n\nAnd now process noise covariance matrix $Q$ could be defined as:\n\n$$ Q = \\begin{bmatrix} (\\sigma_{x''} \\frac{\\Delta t^2}{2})^2 \u0026 \\sigma_{x''} \\frac{\\Delta t^2}{2} \\sigma_{x''} \\Delta t  \\\\\n\\sigma_{x''} \\Delta t \\sigma_{x''} \\frac{\\Delta t^2}{2} \u0026 (\\sigma_{x''} \\Delta t)^2 \\end{bmatrix} = $$\n\n$$ = \\begin{bmatrix} (\\sigma_{x''} \\frac{\\Delta t^2}{2})^2 \u0026 (\\sigma_{x''})^2 \\frac{\\Delta t^2}{2} \\Delta t  \\\\\n(\\sigma_{x''})^2 \\Delta t \\frac{\\Delta t^2}{2} \u0026 (\\sigma_{x''} \\Delta t)^2 \\end{bmatrix} = \\begin{bmatrix} (\\frac{\\Delta t^2}{2})^2 \u0026 \\frac{\\Delta t^2}{2} \\Delta t  \\\\\n\\Delta t \\frac{\\Delta t^2}{2} \u0026 \\Delta t^2 \\end{bmatrix} \\sigma^2_{x''}$$\n\n$$ = \\begin{bmatrix} \\frac{\\Delta t^4}{4} \u0026 \\frac{\\Delta t^3}{2} \\\\\n\\frac{\\Delta t^3}{2} \u0026 \\Delta t^2 \\end{bmatrix} \\sigma^2_{x''} \\tag{24}$$\n\n$$ \\text{Assuming that $x''$ - is acceleration $a$, $Q = \\begin{bmatrix} \\frac{\\Delta t^4}{4} \u0026 \\frac{\\Delta t^3}{2} \\\\\n\\frac{\\Delta t^3}{2} \u0026 \\Delta t^2 \\end{bmatrix} \\sigma^2_{a}$} \\tag{25}$$\n\nCovariance of measurement noise $R$ is scalar (matrix of size $1 \\times 1$) and it is defined as variance of the measurement noise:\n\n$$R = \\begin{matrix}\n\\begin{matrix}\u0026 x\\end{matrix} \\\\\n\\begin{matrix}x\\end{matrix}\n  \\begin{bmatrix}\\sigma^2_{z}\\end{bmatrix}\n \\\\\\\\\n\\end{matrix} = \\sigma^2_{z} \\tag{26}$$\n\nGolang implementation is [here](./kalman_1d.go#L18)\n\nExample of usage:\n```go\nrand.Seed(1337)\ndt := 0.1\nu := 2.0\nstdDevA := 0.25\nstdDevM := 1.2\n\nn := 100\niters := int(float64(n) / dt)\ntrack := make([]struct {\n    t float64\n    x float64\n}, iters)\nv := 0.0\nfor i := 0; i \u003c iters; i++ {\n    track[i] = struct {\n        t float64\n        x float64\n    }{\n        t: v,\n        x: dt * (v*v - v),\n    }\n    v += dt\n}\nkalman := kalman_filter.NewKalman1D(dt, u, stdDevA, stdDevM)\n\nmeasurements := make([]float64, 0, iters)\npredictions := make([]float64, 0, iters)\nfor _, val := range track {\n    // tm := val.t\n    x := val.x\n\n    // Add some noise to perfect track\n    noise := rand.Float64()*100 - 50\n    z := kalman.H.At(0, 0)*x + noise\n    measurements = append(measurements, z)\n\n    // Predict stage\n    kalman.Predict()\n    state := kalman.GetVectorState()\n    predictions = append(predictions, state.At(0, 0))\n\n    // Update stage\n    err := kalman.Update(z)\n    if err != nil {\n        fmt.Println(err)\n        return\n    }\n}\nfmt.Println(\"time;perfect;measurement;prediction\")\nfor i := 0; i \u003c len(track); i++ {\n\tfmt.Printf(\"%f;%f;%f;%f\\n\", track[i].t, track[i].x, measurements[i], predictions[i])\n}\n```\n\nHow exported chart does look like:\n\n\u003cimg src=\"data/kalman-1d.png\" width=\"720\"\u003e\n\n## 2-D Kalman filter\n\nConsidering acceleration motion again let's write down its equations:\n\nConsidering the same physical model as in $(13)$ - $(14)$ let's write down state vector $\\chi_{k}$:\n\n$$\\chi_{k} = \\begin{bmatrix}\nx_{k} \\\\\ny_{k} \\\\\nx'_ {k} \\\\\ny'_ {k} \\end{bmatrix} = \\begin{bmatrix}\nx_{k-1} + x'_ {k-1}\\Delta t + \\frac{x''_ {k-1}(\\Delta t^2)}{2} \\\\\ny_{k-1} + y'_ {k-1}\\Delta t + \\frac{y''_ {k-1}(\\Delta t^2)}{2} \\\\\nx'_ {k-1} + x''_ {k-1}\\Delta t \\\\\ny'_ {k-1} + y''_ {k-1}\\Delta t\n\\end{bmatrix} \\tag{27}$$\n\nMatrix form of $\\chi_{k}$ :\n\n$$\\chi_{k} = \\begin{bmatrix} x_{k} \\\\\ny_{k} \\\\\nx'_ {k} \\\\\ny'_ {k}\n\\end{bmatrix} = \\begin{bmatrix} 1 \u0026 0 \u0026 \\Delta t \u0026 0 \\\\\n0 \u0026 1 \u0026 0 \u0026 \\Delta t \\\\\n0 \u0026 0 \u0026 1 \u0026 0 \\\\\n0 \u0026 0 \u0026 0 \u0026 1 \\end{bmatrix} ⋅ \\begin{bmatrix} x_{k-1} \\\\\ny_{k-1} \\\\\nx'_ {k-1} \\\\\ny'_ {k-1} \\end{bmatrix} + \\begin{bmatrix} \\frac{\\Delta t^2}{2} \u0026 0 \\\\\n0 \u0026 \\frac{\\Delta t^2}{2} \\\\\n\\Delta t \u0026 0 \\\\\n0 \u0026 \\Delta t \\end{bmatrix} ⋅ \\begin{bmatrix} x''_ {k-1} \\\\\ny''_ {k-1} \\end{bmatrix} = $$\n$$ = \\begin{bmatrix} 1 \u0026 0 \u0026 \\Delta t \u0026 0 \\\\\n0 \u0026 1 \u0026 0 \u0026 \\Delta t \\\\\n0 \u0026 0 \u0026 1 \u0026 0 \\\\\n0 \u0026 0 \u0026 0 \u0026 1 \\end{bmatrix} ⋅ \\chi_{k-1} + \\begin{bmatrix} \\frac{\\Delta t^2}{2} \u0026 0 \\\\\n0 \u0026 \\frac{\\Delta t^2}{2} \\\\\n\\Delta t \u0026 0 \\\\\n0 \u0026 \\Delta t \\end{bmatrix} ⋅ \\begin{bmatrix} x''_ {k-1} \\\\\ny''_{k-1} \\end{bmatrix} \\tag{28}$$\n\n$$ \\text{Assuming that $x''$ and $y''$ - is acceleration $a$, }$$\n\n$$ a_{k-1} = \\begin{bmatrix} x''_ {k-1} \\\\\ny''_{k-1} \\end{bmatrix} \\tag{29}$$\n\n$$\\chi_{k} = \\begin{bmatrix} x_{k} \\\\\ny_{k} \\\\\nx'_ {k} \\\\\ny'_ {k}\n\\end{bmatrix} = \\begin{bmatrix} 1 \u0026 0 \u0026 \\Delta t \u0026 0 \\\\\n0 \u0026 1 \u0026 0 \u0026 \\Delta t \\\\\n0 \u0026 0 \u0026 1 \u0026 0 \\\\\n0 \u0026 0 \u0026 0 \u0026 1 \\end{bmatrix} ⋅ \\chi_{k-1} + \\begin{bmatrix} \\frac{\\Delta t^2}{2} \u0026 0 \\\\\n0 \u0026 \\frac{\\Delta t^2}{2} \\\\\n\\Delta t \u0026 0 \\\\\n0 \u0026 \\Delta t \\end{bmatrix} ⋅ a_{k-1} \\tag{30}$$\n\n\nTaking close look on $(16)$ and $(1)$ we can write transition matrix $A$ and control input matrix $B$ as follows:\n\n$$A = \\begin{bmatrix} 1 \u0026 0 \u0026 \\Delta t \u0026 0 \\\\\n0 \u0026 1 \u0026 0 \u0026 \\Delta t \\\\\n0 \u0026 0 \u0026 1 \u0026 0 \\\\\n0 \u0026 0 \u0026 0 \u0026 1 \\end{bmatrix} \\tag{31}$$\n\n$$B = \\begin{bmatrix} \\frac{\\Delta t^2}{2} \u0026 0 \\\\\n0 \u0026 \\frac{\\Delta t^2}{2} \\\\\n\\Delta t \u0026 0 \\\\\n0 \u0026 \\Delta t \\end{bmatrix} \\tag{32}$$\n\nLet's find transformation matrix $H$. According to $(2)$ and $(19)$ - $(20)$:\n\n$$z_{k} = H⋅\\chi_{k} + v_{k} = \\begin{bmatrix} 1 \u0026 0 \u0026 0 \u0026 0 \\\\\n0 \u0026 1 \u0026 0 \u0026 0 \\end{bmatrix} ⋅\\begin{bmatrix} x_{k} \\\\\ny_{k} \\\\\n{x'_  {k}} \\\\\n{y'_ {k}} \\end{bmatrix} + v_{k} \\tag{33}$$\n\n$$ H = \\begin{bmatrix} 1 \u0026 0 \u0026 0 \u0026 0 \\\\\n0 \u0026 1 \u0026 0 \u0026 0 \\end{bmatrix} \\tag{34}$$\n\nProcess noise covariance matrix $Q$:\n\n$$Q = \\begin{matrix}\n \u0026 \\begin{matrix}x \u0026\u0026 y \u0026\u0026 x' \u0026\u0026 y'\\end{matrix} \\\\\n\\begin{matrix}x \\\\\ny \\\\\nx' \\\\\ny'\\end{matrix} \u0026 \n  \\begin{bmatrix} \\sigma^2_{x} \u0026 0 \u0026 \\sigma_{x} \\sigma_{x'} \u0026 0 \\\\\n0 \u0026 \\sigma^2_{y} \u0026 0 \u0026 \\sigma_{y} \\sigma_{y'} \\\\\n\\sigma_{x'} \\sigma_{x} \u0026 0 \u0026 \\sigma^2_{x'} \u0026 0 \\\\\n0 \u0026 \\sigma_{y'} \\sigma_{y} \u0026 0 \u0026 \\sigma^2_{y'}\\end{bmatrix}\n \\\\\\\\\n\\end{matrix} \\tag{35}$$\n\n$$\\text{, where} $$\n\n$$ \\text{$\\sigma_{x}$ - standart deviation of position for $x$ component} $$\n\n$$ \\text{$\\sigma_{y}$ - standart deviation of position for $y$ component} $$\n\n$$ \\text{$\\sigma_{x'}$ - standart deviation of velocity for $x$ component} $$\n\n$$ \\text{$\\sigma_{y'}$ - standart deviation of velocity for $y$ component} $$\n\nSince we know about $(14)$ we can define $\\sigma_{x}$, $\\sigma_{y}$, $\\sigma_{x'}$ and $\\sigma_{y'}$ as:\n\n$$ \\sigma_{x} = \\sigma_{x''} \\frac{\\Delta t^2}{2} \\tag{36}$$\n\n$$ \\sigma_{y} = \\sigma_{y''} \\frac{\\Delta t^2}{2} \\tag{37}$$\n\n$$ \\sigma_{x'} = \\sigma_{x''} \\Delta t \\tag{38}$$\n\n$$ \\sigma_{y'} = \\sigma_{y''} \\Delta t \\tag{39}$$\n\n$$\\text{, where $\\sigma_{x''}$ and $\\sigma_{y''}$ - standart deviation of acceleration (tuned values)} $$\n\nAnd now process noise covariance matrix $Q$ could be defined as:\n\n$$ Q = \\begin{bmatrix} (\\sigma_{x''} \\frac{\\Delta t^2}{2})^2 \u0026 0 \u0026 \\sigma_{x''} \\frac{\\Delta t^2}{2} \\sigma_{x''} \\Delta t \u0026 0 \\\\\n0 \u0026 (\\sigma_{y''} \\frac{\\Delta t^2}{2})^2 \u0026 0 \u0026 \\sigma_{y''} \\frac{\\Delta t^2}{2} \\sigma_{y''} \\Delta t \\\\\n\\sigma_{x''} \\frac{\\Delta t^2}{2} \\sigma_{x''} \\Delta t \u0026 0 \u0026 (\\sigma_{x''} \\Delta t)^2 \u0026 0 \\\\\n0 \u0026 \\sigma_{y''} \\frac{\\Delta t^2}{2} \\sigma_{y''} \\Delta t \u0026 0 \u0026 (\\sigma_{y''} \\Delta t)^2 \\end{bmatrix} = $$\n\n$$ = \\begin{bmatrix} (\\sigma_{x''} \\frac{\\Delta t^2}{2})^2 \u0026 0 \u0026 (\\sigma_{x''})^2 \\frac{\\Delta t^2}{2} \\Delta t \u0026 0 \\\\\n0 \u0026 (\\sigma_{y''} \\frac{\\Delta t^2}{2})^2 \u0026 0 \u0026 (\\sigma_{y''})^2 \\frac{\\Delta t^2}{2} \\Delta t \\\\\n(\\sigma_{x''})^2 \\frac{\\Delta t^2}{2} \\Delta t \u0026 0 \u0026 (\\sigma_{x''} \\Delta t)^2 \u0026 0 \\\\\n0 \u0026 (\\sigma_{y''})^2 \\frac{\\Delta t^2}{2}\\Delta t \u0026 0 \u0026 (\\sigma_{y''} \\Delta t)^2 \\end{bmatrix} = \\text{| Knowing that $x''$ and $y''$ - acceleration|} = $$ \n$$ = \\begin{bmatrix} (\\frac{\\Delta t^2}{2})^2 \u0026 0 \u0026 \\frac{\\Delta t^2}{2} \\Delta t \u0026 0 \\\\\n0 \u0026 (\\frac{\\Delta t^2}{2})^2 \u0026 0 \u0026 \\frac{\\Delta t^2}{2} \\Delta t \\\\\n\\frac{\\Delta t^2}{2} \\Delta t \u0026 0 \u0026 \\Delta t^2 \u0026 0 \\\\\n0 \u0026 \\Delta t \\frac{\\Delta t^2}{2} \u0026 0 \u0026 \\Delta t^2 \\end{bmatrix} \\sigma^2_{a''}$$\n\n$$ = \\begin{bmatrix} \\frac{\\Delta t^4}{4} \u0026 0 \u0026 \\frac{\\Delta t^3}{2} \u0026 0 \\\\\n0 \u0026 \\frac{\\Delta t^4}{4} \u0026 0 \u0026 \\frac{\\Delta t^3}{2} \\\\\n\\frac{\\Delta t^3}{2} \u0026 0 \u0026 \\Delta t^2 \u0026 0 \\\\\n0 \u0026 \\frac{\\Delta t^3}{2} \u0026 0 \u0026 \\Delta t^2 \\end{bmatrix} \\sigma^2_{a''} \\tag{40}$$\n\nCovariance of measurement noise $R$ is matrix of size $2 \\times 2$ (since there are two components - $x$ and $y$) and it is defined as variance of the measurement noise:\n\n$$R = \\begin{matrix}\n\\begin{matrix}\u0026 x \u0026 y\\end{matrix} \\\\\n\\begin{matrix}x \\\\\ny \\end{matrix}\n  \\begin{bmatrix}\\sigma^2_{x} \u0026 0 \\\\\n  0 \u0026 \\sigma^2_{y} \\end{bmatrix}\n \\\\\\\\\n\\end{matrix} = \\begin{bmatrix}\\sigma^2_{x} \u0026 0 \\\\\n  0 \u0026 \\sigma^2_{y} \\end{bmatrix} \\tag{41}$$\n\nGolang implementation is [here](./kalman_2d.go#L20)\n\nExample of usage:\n```go\ndt := 0.04 // 1/25 = 25 fps - just an example\nux := 1.0\nuy := 1.0\nstdDevA := 2.0\nstdDevMx := 0.1\nstdDevMy := 0.1\n\n// Sample measurements\n// Note: in this example Y-axis going from up to down\nxs := []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}\nys := []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}\n\n// Assume that initial X,Y coordinates match the first measurement\nix := xs[0] // Initial state for X\niy := ys[0] // Initial state for Y\n\nkalman := kalman_filter.NewKalman2D(dt, ux, uy, stdDevA, stdDevMx, stdDevMy, kalman_filter.WithState2D(ix, iy))\n\npredictions := make([][]float64, 0, len(xs))\nupdatedStates := make([][]float64, 0, len(xs))\nfor i := 0; i \u003c len(xs); i++ {\n    // Considering that the measurements are noisy\n    mx := xs[i]\n    my := ys[i]\n\n    // Predict stage\n    kalman.Predict()\n    state := kalman.GetVectorState()\n    predictions = append(predictions, []float64{state.At(0, 0), state.At(1, 0)})\n\n    // Update stage\n    err := kalman.Update(mx, my)\n    if err != nil {\n        fmt.Println(err)\n        return\n    }\n    updatedState := kalman.GetVectorState()\n    updatedStates = append(updatedStates, []float64{updatedState.At(0, 0), updatedState.At(1, 0)})\n}\nfmt.Println(\"measurement X;measurement Y;prediction X;prediction Y;updated X;updated Y\")\nfor i := 0; i \u003c len(xs); i++ {\n    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])\n}\n```\n\nHow exported chart does look like:\n\n\u003cimg src=\"data/kalman-2d.png\" width=\"720\"\u003e\n\n## Support and contribution\n\nIf you have troubles or questions please [open an issue](https://github.com/LdDl/kalman-filter/issues/new).\n\nPR's are welcome.\n\n## Dependencies\n* Matrix computations - [gonum](https://github.com/gonum/gonum#gonum). License is **BSD 3-Clause \"New\" or \"Revised\" License**. [Link](https://github.com/gonum/gonum/blob/master/LICENSE)\n* Errors wraping - [errors](https://github.com/pkg/errors#errors-----). License is **BSD 2-Clause \"Simplified\" License**. [Link](https://github.com/pkg/errors/blob/master/LICENSE)\n\n## License\nLicense of this library is [MIT](https://en.wikipedia.org/wiki/MIT_License).\n\nYou can check it [here](LICENSE.md)\n\n## Developers\n\nLdDl https://github.com/LdDl\n\nPavel7824 https://github.com/Pavel7824\n\n## References\n* [Greg Welch and Gary Bishop, ‘An Introduction to the Kalman Filter’, July 24, 2006](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf)\n* [Introducion to the Kalman Filter by Alex Becker](https://www.kalmanfilter.net/default.aspx)\n* [Kalman filter on wikipedia](https://en.wikipedia.org/wiki/Kalman_filter)\n* [State-transition matrix](https://en.wikipedia.org/wiki/State-transition_matrix)\n* [Python implementation by Rahmad Sadli](https://machinelearningspace.com/object-tracking-python/)\n\n# P.S.\nI did struggle on displaying matrices in GitHub's MathJax markdown. If you know better way to do it you are welcome\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Flddl%2Fkalman-filter","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Flddl%2Fkalman-filter","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Flddl%2Fkalman-filter/lists"}