Created
April 23, 2015 14:43
-
-
Save zpconn/8ae9bff5153aa5f44465 to your computer and use it in GitHub Desktop.
Simple Kalman-type filter for position tracking
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| <!DOCTYPE html> | |
| <html> | |
| <head> | |
| <style type="text/css"> | |
| html, | |
| body { | |
| margin: 0; | |
| overflow: hidden; | |
| height: 100%; | |
| } | |
| /* Scale canvas with resize attribute to full size */ | |
| canvas[resize] { | |
| width: 100%; | |
| height: 100%; | |
| } | |
| </style> | |
| <script type="text/javascript" src="paper-full.js"></script> | |
| <script type="text/paperscript" canvas="myCanvas"> | |
| var myPath = new Path(); | |
| myPath.strokeColor = 'black'; | |
| var filteredPath = new Path(); | |
| filteredPath.strokeColor = 'red'; | |
| var minAccuracy = 1; | |
| var Q = 0.1; | |
| var timestamp = 0; | |
| var x = 0; | |
| var oldx = 0; | |
| var y = 0; | |
| var oldy = 0; | |
| var variance = -1; | |
| function findAngle(p0,p1,c) { | |
| var p0c = Math.sqrt(Math.pow(c.x-p0.x,2)+ | |
| Math.pow(c.y-p0.y,2)); // p0->c (b) | |
| var p1c = Math.sqrt(Math.pow(c.x-p1.x,2)+ | |
| Math.pow(c.y-p1.y,2)); // p1->c (a) | |
| var p0p1 = Math.sqrt(Math.pow(p1.x-p0.x,2)+ | |
| Math.pow(p1.y-p0.y,2)); // p0->p1 (c) | |
| return Math.acos((p1c*p1c+p0c*p0c-p0p1*p0p1)/(2*p1c*p0c)); | |
| } | |
| // This function is called whenever the user | |
| // clicks the mouse in the view: | |
| function onMouseDown(event) { | |
| // Add a segment to the path at the position of the mouse: | |
| myPath.add(event.point); | |
| console.log(event.point); | |
| var angle = findAngle({x: x, y: y}, | |
| {x: event.point.x, y: event.point.y}, | |
| {x: oldx, y: oldy}); | |
| var a = (angle / Math.PI) + 1.0; | |
| a = a * a; | |
| var accuracy = a - 1.0; | |
| //var accuracy = (angle / Math.PI) * (angle / Math.PI); | |
| if (isNaN(accuracy)) { | |
| accuracy = 0.1; | |
| } | |
| console.log('angle: ' + angle); | |
| console.log('accuracy: ' + accuracy); | |
| oldx = x; | |
| oldy = y; | |
| if (variance < 0) { | |
| x = event.point.x; | |
| y = event.point.y; | |
| variance = accuracy * accuracy; | |
| timestamp = (new Date).getTime(); | |
| } else { | |
| var dt = (new Date).getTime() - timestamp; | |
| if (dt > 0) { | |
| variance += dt * Q * Q / 1000; | |
| timestamp += dt; | |
| } | |
| var K = variance / (variance + accuracy * accuracy); | |
| x += K * (event.point.x - x); | |
| y += K * (event.point.y - y); | |
| variance = (1 - K) * variance; | |
| } | |
| filteredPath.add({'x': x, 'y': y}); | |
| } | |
| </script> | |
| </head> | |
| <body> | |
| <canvas id="myCanvas" resize="true"></canvas> | |
| </body> | |
| </html> |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment