/// <summary>
/// モデル状態を修正する. 修正された状態を kalman->state_post に保存し，これを出力として返す．
/// </summary>
/// <param name="kalman">更新されるカルマンフィルタ状態</param>
/// <param name="measurement">観測ベクトルを含むCvMat</param>
/// <returns>修正された状態を kalman->state_post に保存し，これを出力として返す．</returns>
#else
/// <summary>
/// Adjusts model state
/// </summary>
/// <param name="kalman">Kalman filter</param>
/// <param name="measurement">CvMat containing the measurement vector. </param>
/// <returns>The function stores adjusted state at kalman->state_post and returns it on output.</returns>
#endif
public static CvMat KalmanCorrect(CvKalman kalman, CvMat measurement)
{
if (kalman == null)
throw new ArgumentNullException("kalman");
if (measurement == null)
throw new ArgumentNullException("measurement");
IntPtr result = CvInvoke.cvKalmanCorrect(kalman.CvPtr, measurement.CvPtr);
return new CvMat(result, false);
}
/// <summary>
/// 次のモデル状態を推定する
/// </summary>
/// <param name="kalman">カルマンフィルタ状態</param>
/// <param name="control">コントロールベクトル (uk)．外部コントロールが存在しない場合（control_params=0）に限り，null である．</param>
/// <returns></returns>
#else
/// <summary>
/// Estimates subsequent model state
/// </summary>
/// <param name="kalman">Kalman filter state. </param>
/// <param name="control">Control vector (uk), should be null iff there is no external control (control_params=0).</param>
/// <returns>The function returns the estimated state. </returns>
#endif
public static CvMat KalmanPredict(CvKalman kalman, CvMat control)
{
if (kalman == null)
throw new ArgumentNullException("kalman");
IntPtr controlPtr = (control == null) ? IntPtr.Zero : control.CvPtr;
IntPtr result = NativeMethods.cvKalmanPredict(kalman.CvPtr, controlPtr);
if (result == IntPtr.Zero)
return null;
else
return new CvMat(result, false);
}
/// <summary>
/// モデル状態を修正する. 修正された状態を kalman->state_post に保存し，これを出力として返す．
/// </summary>
/// <param name="kalman">更新されるカルマンフィルタ状態</param>
/// <param name="measurement">観測ベクトルを含むCvMat</param>
/// <returns>修正された状態を kalman->state_post に保存し，これを出力として返す．</returns>
#else
/// <summary>
/// Adjusts model state
/// </summary>
/// <param name="kalman">Kalman filter</param>
/// <param name="measurement">CvMat containing the measurement vector. </param>
/// <returns>The function stores adjusted state at kalman->state_post and returns it on output.</returns>
#endif
public static CvMat KalmanCorrect(CvKalman kalman, CvMat measurement)
{
if (kalman == null)
{
throw new ArgumentNullException("kalman");
}
if (measurement == null)
{
throw new ArgumentNullException("measurement");
}
IntPtr result = NativeMethods.cvKalmanCorrect(kalman.CvPtr, measurement.CvPtr);
return(new CvMat(result, false));
}
/// <summary>
/// 次のモデル状態を推定する
/// </summary>
/// <param name="kalman">カルマンフィルタ状態</param>
/// <param name="control">コントロールベクトル (uk)．外部コントロールが存在しない場合（control_params=0）に限り，null である．</param>
/// <returns></returns>
#else
/// <summary>
/// Estimates subsequent model state
/// </summary>
/// <param name="kalman">Kalman filter state. </param>
/// <param name="control">Control vector (uk), should be null iff there is no external control (control_params=0).</param>
/// <returns>The function returns the estimated state. </returns>
#endif
public static CvMat KalmanPredict(CvKalman kalman, CvMat control)
{
if (kalman == null)
{
throw new ArgumentNullException("kalman");
}
IntPtr controlPtr = (control == null) ? IntPtr.Zero : control.CvPtr;
IntPtr result = NativeMethods.cvKalmanPredict(kalman.CvPtr, controlPtr);
if (result == IntPtr.Zero)
{
return(null);
}
else
{
return(new CvMat(result, false));
}
}
// Set up the initial values for the Kalman filter
void InitializeKalmanFilter()
{
// Create the Kalman Filter
_kalman = Cv.CreateKalman(4, 2, 0);
// Set the Kalman filter initial state
_kalman.StatePre.Set2D(0, 0, _rectToTrack.X); // Initial X position
_kalman.StatePre.Set2D(1, 0, _rectToTrack.Y); // Initial Y position
_kalman.StatePre.Set2D(2, 0, 0); // Initial X velocity
_kalman.StatePre.Set2D(3, 0, 0); // Initial Y velocity
// Prediction Equations
// X position is a function of previous X positions and previous X velocities
_kalman.TransitionMatrix.Set2D(0, 0, 1);
_kalman.TransitionMatrix.Set2D(1, 0, 0);
_kalman.TransitionMatrix.Set2D(2, 0, 1);
_kalman.TransitionMatrix.Set2D(3, 0, 0);
// Y position is a function of previous Y positions and previous Y velocities
_kalman.TransitionMatrix.Set2D(0, 1, 0);
_kalman.TransitionMatrix.Set2D(1, 1, 1);
_kalman.TransitionMatrix.Set2D(2, 1, 0);
_kalman.TransitionMatrix.Set2D(3, 1, 1);
// X velocity is a function of previous X velocities
_kalman.TransitionMatrix.Set2D(0, 2, 0);
_kalman.TransitionMatrix.Set2D(1, 2, 0);
_kalman.TransitionMatrix.Set2D(2, 2, 1);
_kalman.TransitionMatrix.Set2D(3, 2, 0);
// Y velocity is a function of previous Y velocities
_kalman.TransitionMatrix.Set2D(0, 3, 0);
_kalman.TransitionMatrix.Set2D(1, 3, 0);
_kalman.TransitionMatrix.Set2D(2, 3, 0);
_kalman.TransitionMatrix.Set2D(3, 3, 1);
// set Kalman Filter
Cv.SetIdentity(_kalman.MeasurementMatrix, 1.0f);
Cv.SetIdentity(_kalman.ProcessNoiseCov, 0.4f); //0.4
Cv.SetIdentity(_kalman.MeasurementNoiseCov, 3f); //3
Cv.SetIdentity(_kalman.ErrorCovPost, 1.0f);
}
public unsafe Kalman()
{
// cvKalmanPredict, cvKalmanCorrect
// カルマンフィルタを用いて回転する点を追跡する
// A matrix data
float[] A = new float[] { 1, 1, 0, 1 };
using (IplImage img = new IplImage(500, 500, BitDepth.U8, 3))
using (CvKalman kalman = new CvKalman(2, 1, 0))
using (CvWindow window = new CvWindow("Kalman", WindowMode.AutoSize))
{
// state is (phi, delta_phi) - angle and angle increment
CvMat state = new CvMat(2, 1, MatrixType.F32C1);
CvMat process_noise = new CvMat(2, 1, MatrixType.F32C1);
// only phi (angle) is measured
CvMat measurement = new CvMat(1, 1, MatrixType.F32C1);
measurement.SetZero();
CvRandState rng = new CvRandState(0, 1, -1, DistributionType.Uniform);
int code = -1;
for (; ; )
{
Cv.RandSetRange(rng, 0, 0.1, 0);
rng.DistType = DistributionType.Normal;
Marshal.Copy(A, 0, kalman.TransitionMatrix.Data, A.Length);
kalman.MeasurementMatrix.SetIdentity(1);
kalman.ProcessNoiseCov.SetIdentity(1e-5);
kalman.MeasurementNoiseCov.SetIdentity(1e-1);
kalman.ErrorCovPost.SetIdentity(1);
// choose random initial state
Cv.Rand(rng, kalman.StatePost);
rng.DistType = DistributionType.Normal;
for (; ; )
{
float state_angle = state.DataSingle[0];
CvPoint state_pt = CalcPoint(img, state_angle);
// predict point position
CvMat prediction = kalman.Predict(null);
float predict_angle = prediction.DataSingle[0];
CvPoint predict_pt = CalcPoint(img, predict_angle);
Cv.RandSetRange(rng, 0, Math.Sqrt(kalman.MeasurementNoiseCov.DataSingle[0]), 0);
Cv.Rand(rng, measurement);
// generate measurement
Cv.MatMulAdd(kalman.MeasurementMatrix, state, measurement, measurement);
float measurement_angle = measurement.DataArraySingle[0];
CvPoint measurement_pt = CalcPoint(img, measurement_angle);
img.SetZero();
DrawCross(img, state_pt, CvColor.White, 3);
DrawCross(img, measurement_pt, CvColor.Red, 3);
DrawCross(img, predict_pt, CvColor.Green, 3);
img.Line(state_pt, measurement_pt, new CvColor(255, 0, 0), 3, LineType.AntiAlias, 0);
img.Line(state_pt, predict_pt, new CvColor(255, 255, 0), 3, LineType.AntiAlias, 0);
// adjust Kalman filter state
kalman.Correct(measurement);
Cv.RandSetRange(rng, 0, Math.Sqrt(kalman.ProcessNoiseCov.DataSingle[0]), 0);
Cv.Rand(rng, process_noise);
Cv.MatMulAdd(kalman.TransitionMatrix, state, process_noise, state);
window.ShowImage(img);
// break current simulation by pressing a key
code = CvWindow.WaitKey(100);
if (code > 0)
{
break;
}
}
// exit by ESCAPE
if (code == 27)
{
break;
}
}
}
}
/// <summary>
/// 次のモデル状態を推定する.
/// </summary>
/// <param name="kalman">カルマンフィルタ状態</param>
/// <returns></returns>
#else
/// <summary>
/// Estimates subsequent model state
/// </summary>
/// <param name="kalman">Kalman filter state. </param>
/// <returns>The function returns the estimated state. </returns>
#endif
public static CvMat KalmanPredict(CvKalman kalman)
{
return KalmanPredict(kalman, null);
}
/// <summary>
/// モデル状態を修正する. 修正された状態を kalman->state_post に保存し，これを出力として返す．cvKalmanCorrectのエイリアス.
/// </summary>
/// <param name="kalman">更新されるカルマンフィルタ状態</param>
/// <param name="measurement">観測ベクトルを含むCvMat</param>
/// <returns>修正された状態を kalman->state_post に保存し，これを出力として返す．</returns>
#else
/// <summary>
/// Adjusts model state
/// </summary>
/// <param name="kalman">Kalman filter</param>
/// <param name="measurement">CvMat containing the measurement vector. </param>
/// <returns>The function stores adjusted state at kalman->state_post and returns it on output.</returns>
#endif
public static CvMat KalmanUpdateByMeasurement(CvKalman kalman, CvMat measurement)
{
return KalmanCorrect(kalman, measurement);
}
/// <summary>
/// 次のモデル状態を推定する. cvKalmanPredictのエイリアス.
/// </summary>
/// <param name="kalman">カルマンフィルタ状態</param>
/// <param name="control">コントロールベクトル (uk)．外部コントロールが存在しない場合（control_params=0）に限り，null である．</param>
/// <returns></returns>
#else
/// <summary>
/// Estimates subsequent model state
/// </summary>
/// <param name="kalman">Kalman filter state. </param>
/// <param name="control">Control vector (uk), should be null iff there is no external control (control_params=0).</param>
/// <returns>The function returns the estimated state. </returns>
#endif
public static CvMat KalmanUpdateByTime(CvKalman kalman, CvMat control)
{
return KalmanPredict(kalman, control);
}
/// <summary>
/// 次のモデル状態を推定する. cvKalmanPredictのエイリアス.
/// </summary>
/// <param name="kalman">カルマンフィルタ状態</param>
/// <returns></returns>
#else
/// <summary>
/// Estimates subsequent model state
/// </summary>
/// <param name="kalman">Kalman filter state. </param>
/// <returns>The function returns the estimated state. </returns>
#endif
public static CvMat KalmanUpdateByTime(CvKalman kalman)
{
return KalmanPredict(kalman);
}
/// <summary>
/// 次のモデル状態を推定する.
/// </summary>
/// <param name="kalman">カルマンフィルタ状態</param>
/// <returns></returns>
#else
/// <summary>
/// Estimates subsequent model state
/// </summary>
/// <param name="kalman">Kalman filter state. </param>
/// <returns>The function returns the estimated state. </returns>
#endif
public static CvMat KalmanPredict(CvKalman kalman)
{
return(KalmanPredict(kalman, null));
}
/// <summary>
/// モデル状態を修正する. 修正された状態を kalman->state_post に保存し，これを出力として返す．cvKalmanCorrectのエイリアス.
/// </summary>
/// <param name="kalman">更新されるカルマンフィルタ状態</param>
/// <param name="measurement">観測ベクトルを含むCvMat</param>
/// <returns>修正された状態を kalman->state_post に保存し，これを出力として返す．</returns>
#else
/// <summary>
/// Adjusts model state
/// </summary>
/// <param name="kalman">Kalman filter</param>
/// <param name="measurement">CvMat containing the measurement vector. </param>
/// <returns>The function stores adjusted state at kalman->state_post and returns it on output.</returns>
#endif
public static CvMat KalmanUpdateByMeasurement(CvKalman kalman, CvMat measurement)
{
return(KalmanCorrect(kalman, measurement));
}
/// <summary>
/// 次のモデル状態を推定する. cvKalmanPredictのエイリアス.
/// </summary>
/// <param name="kalman">カルマンフィルタ状態</param>
/// <param name="control">コントロールベクトル (uk)．外部コントロールが存在しない場合（control_params=0）に限り，null である．</param>
/// <returns></returns>
#else
/// <summary>
/// Estimates subsequent model state
/// </summary>
/// <param name="kalman">Kalman filter state. </param>
/// <param name="control">Control vector (uk), should be null iff there is no external control (control_params=0).</param>
/// <returns>The function returns the estimated state. </returns>
#endif
public static CvMat KalmanUpdateByTime(CvKalman kalman, CvMat control)
{
return(KalmanPredict(kalman, control));
}
/// <summary>
/// 次のモデル状態を推定する. cvKalmanPredictのエイリアス.
/// </summary>
/// <param name="kalman">カルマンフィルタ状態</param>
/// <returns></returns>
#else
/// <summary>
/// Estimates subsequent model state
/// </summary>
/// <param name="kalman">Kalman filter state. </param>
/// <returns>The function returns the estimated state. </returns>
#endif
public static CvMat KalmanUpdateByTime(CvKalman kalman)
{
return(KalmanPredict(kalman));
}
