- OpenCVDemo3.zip
- OpenCVDemo
- Description.odt
- OpenCv-2.3.1-x86
- opencv_calib3d231.dll
- opencv_contrib231.dll
- opencv_core231.dll
- opencv_features2d231.dll
- opencv_ffmpeg.dll
- opencv_flann231.dll
- opencv_gpu231.dll
- opencv_highgui231.dll
- opencv_imgproc231.dll
- opencv_legacy231.dll
- opencv_ml231.dll
- opencv_objdetect231.dll
- opencv_ts231.dll
- opencv_video231.dll
- tbb.dll
- OpenCVDemo.sln
- OpenCvSharp-2.3.1-x86-20111030
- TrackingEyes.jpg
- WindowsFormsApplication1
- OpenCVDemo3-noexe.zip
|
///////////////////////////////////////////////////////////////////////////////
//
// PositionFilter.cs
//
// By Philip R. Braica (HoshiKata@aol.com, VeryMadSci@gmail.com)
//
// Distributed under the The Code Project Open License (CPOL)
// http://www.codeproject.com/info/cpol10.aspx
///////////////////////////////////////////////////////////////////////////////
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
namespace OpenCVDemo
{
/// <summary>
/// Kalman filter, 2D state, 1D measurement.
///
/// Algorithm:
/// X = Predicted
/// P = Covariance
/// Y = Residual
/// M = Measured
/// S = Residual Covariance
/// R = Minimal inovative covariance.
/// K = Kalman gain
/// Q = Minimal update covariance of P
/// H = Rolls actual to predicted: Measured = H*X+ + noise.
/// I = Identity matrix.
///
/// Move prediction from old time to new time.
/// X = F*X + B*U
/// P = F*X*F^T + Q
///
/// Update:
/// Y = M - H*X
/// S = H*P*H^T + R
/// K = P * H^T * S^-1
/// X = X + K * Y upates to this time.
/// P = (I - K * H) * P update to this time.
///
/// Example:
/// X = x, v position and velocity.
/// Predict
/// x = = F * X + H * U
/// F = [1, dt], [0, 1],
/// H * U = 0 'cause no controlled inputs.
/// Q = [min x to x cov., min vel to x cov.], [min pos to vel cov., min vel to vel cov.]
/// H = [1, 0] => we measure only the 1 value.
/// R = Expectation of measurement noise.
/// Initialize P always to a diagonal large matrix.
/// </summary>
public class KalmanFilter
{
/// <summary>
/// Constructor.
/// </summary>
public KalmanFilter()
{
Reset();
}
#region Protected stuff: m_x, m_v, m_p, m_q, m_r.
/// <summary>
/// Position.
/// </summary>
protected double m_x = 0;
/// <summary>
/// Velocity.
/// </summary>
protected double m_v = 0;
/// <summary>
/// Covariance vector.
/// </summary>
protected double[] m_p = new double[4];
/// <summary>
/// Minimal prediction covariance.
/// </summary>
protected double[] m_q = new double[2];
/// <summary>
/// Measurement covariance.
/// </summary>
protected double m_r = 0;
#endregion
/// <summary>
/// Variance.
/// </summary>
public double Variance
{ get { return System.Math.Abs(m_p[0]); } }
/// <summary>
/// The current value after last update.
/// </summary>
public double Value
{ get { return m_x; } }
/// <summary>
/// Setup the filter.
/// </summary>
public void Reset()
{
m_q[0] = 50; // Predicted initial position covariance in pixels.
m_q[1] = 30; // Predicted initial velocity covariance in pixels.
m_r = 5; // Measure to measure covariance
m_p[0] = 10000000;
m_p[1] = 0;
m_p[2] = 0;
m_p[3] = 10000000;
m_x = 0;
m_v = 0;
}
/// <summary>
/// Reset to an initial position.
/// </summary>
/// <param name="x"></param>
public void Reset(int x)
{
Reset();
m_x = x;
}
/// <summary>
/// Update the value.
/// </summary>
/// <param name="m">Measurement.</param>
public double Update(double m)
{
// Move prediction from old time to new time.
// X = F*X + B*U
// P = F*X*F^T + Q
//
// Update:
// Y = M - H*X
// S = H*P*H^T + R
// K = P * H^T * S^-1
// X = X + K * Y upates to this time.
// P = (I - K * H) * P update to this time.
// X = F*X + B*U
double old = m_x;
m_x += m_v;
// P = F*P*F^T + Q
// = [1 t] [a b] [1 0]
// [0 1] [c d] [t 1]
// = [a + ct, b + t] [1 0]
// = [ c, d] [t 1]
// = [ac + act + bt + tt, b + t]
// [ c + dt, d]
// = [ac(1+t) + (b+t)t, b + t]
// [ c + dt, d]
m_p[0] = ((m_p[0] * m_p[2]) * (2)) + (m_p[1] + 1);
m_p[1] += 1;
m_p[2] += 1;
// Y = M - H*X
double y0 = m - m_x; // y[1] is always 0 because there is no velocity measurement.
// S = H*P*H^T + R
double s = m_p[0] + m_r;
// K = P * H^T * S^-1
double k = m_p[0] / s;
// X = X + K * Y upates to this time.
m_x += y0 * k;
m_v += k * (m - old);
// P = (I - K * H) * P update to this time.
m_p[0] = (1 - k) * m_p[0]; // 'cause H = [1, 0].
return m_x;
}
}
/// <summary>
/// Kalman filter of a rectangle.
/// </summary>
public class PositionFilter
{
/// <summary>
/// X position.
/// </summary>
KalmanFilter m_X = new KalmanFilter();
/// <summary>
/// Y position.
/// </summary>
KalmanFilter m_Y = new KalmanFilter();
/// <summary>
/// Just use last scalar for width.
/// </summary>
public int Width { get; set; }
/// <summary>
/// Just use last scalar for height.
/// </summary>
public int Height { get; set; }
/// <summary>
/// X position.
/// </summary>
public int X { get { return (int)m_X.Value; } }
/// <summary>
/// Y position.
/// </summary>
public int Y { get { return (int)m_Y.Value; } }
/// <summary>
/// The location.
/// </summary>
public Point Location { get { return new Point(X, Y); } }
/// <summary>
/// Current bounds value.
/// </summary>
public Rectangle Bounds
{
get
{
return new Rectangle(
(int)m_X.Value,
(int)m_Y.Value,
Width,
Height);
}
}
/// <summary>
/// Get estimated search region.
/// </summary>
public Rectangle SearchBounds
{
get
{
int dx = (int)(m_X.Variance*4 + 10);
int dy = (int)(m_Y.Variance*4 + 10);
return new Rectangle(
(int)(m_X.Value - dx),
(int)(m_Y.Value - dy),
Width + dx + dx,
Height + dy + dy);
}
}
/// <summary>
/// Variance.
/// </summary>
public double Variance
{ get { return (m_X.Variance + m_Y.Variance) / 2; } }
/// <summary>
/// Setup the filter.
/// </summary>
public void Reset()
{
m_X.Reset();
m_Y.Reset();
}
public void Reset(int x, int y)
{
m_X.Reset(x);
m_X.Reset(y);
}
/// <summary>
/// Update the value w/ constant time between
/// measurements.
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public Rectangle Update(Rectangle m)
{
m_X.Update(m.X);
m_Y.Update(m.Y);
Width = m.Width;
Height = m.Height;
return Bounds;
}
}
}
|
By viewing downloads associated with this article you agree to the Terms of Service and the article's licence.
If a file you wish to view isn't highlighted, and is a text file (not binary), please
let us know and we'll add colourisation support for it.
Phil is a Principal Software developer focusing on weird yet practical algorithms that run the gamut of embedded and desktop (PID loops, Kalman filters, FFTs, client-server SOAP bindings, ASIC design, communication protocols, game engines, robotics).
In his personal life he is a part time mad scientist, full time dad, and studies small circle jujitsu, plays guitar and piano.