cv::KalmanFilter の使い方【OpenCV/C++】〜カルマンフィルタで物体追跡〜
cv::KalmanFilter は、ノイズを含む観測値から状態の最良推定値を求めるフィルタです。物体追跡において「検出が1フレーム途切れた」「座標がジッタする」といった問題をシンプルなコードで解消できます。
最小の構成イメージ:
cv::KalmanFilter kf(4, 2, 0); // 状態4次元(x,y,vx,vy), 観測2次元(x,y)
cv::Mat pred = kf.predict(); // 次フレームの状態を予測
cv::Mat meas = (cv::Mat_<float>(2,1) << mx, my);
cv::Mat est = kf.correct(meas); // 観測で補正 → 最良推定値
動作環境
- OpenCV 4.x
- コンパイル:
g++ -std=c++17 main.cpp $(pkg-config --cflags --libs opencv4) -o main
環境構築がまだの方は環境構築ガイドを先にどうぞ。
基本の使い方
円軌道上を動く点(真値)にガウスノイズを加えた「観測点」をカルマンフィルタで平滑化する最小サンプルです。
#include <opencv2/opencv.hpp>
#include <iostream>
#include <random>
int main()
{
// --- カルマンフィルタの初期化 ---
// 状態ベクトル: [x, y, vx, vy] (位置 + 速度)
// 観測ベクトル: [x, y]
const int stateSize = 4;
const int measSize = 2;
const int ctrlSize = 0;
cv::KalmanFilter kf(stateSize, measSize, ctrlSize, CV_32F);
// 状態遷移行列 A: 等速直線運動モデル
// x' = x + vx*dt, y' = y + vy*dt (dt=1フレーム)
kf.transitionMatrix = (cv::Mat_<float>(4, 4) <<
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1);
// 観測行列 H: 状態から観測への射影 (x,y だけ取り出す)
kf.measurementMatrix = (cv::Mat_<float>(2, 4) <<
1, 0, 0, 0,
0, 1, 0, 0);
// プロセスノイズ共分散 Q (小さいほど予測モデルを信頼)
cv::setIdentity(kf.processNoiseCov, cv::Scalar::all(1e-4));
// 観測ノイズ共分散 R (大きいほど観測を疑う)
cv::setIdentity(kf.measurementNoiseCov, cv::Scalar::all(1e-1));
// 事後誤差共分散の初期値
cv::setIdentity(kf.errorCovPost, cv::Scalar::all(1));
// 状態初期値 (画面中央)
kf.statePost = (cv::Mat_<float>(4, 1) << 320, 240, 0, 0);
// --- シミュレーション: 円軌道 + ガウスノイズ ---
std::mt19937 rng(42);
std::normal_distribution<float> noise(0.0f, 10.0f); // σ=10px
cv::Mat canvas(480, 640, CV_8UC3);
cv::Mat meas(measSize, 1, CV_32F);
std::cout << "frame, true_x, true_y, obs_x, obs_y, est_x, est_y\n";
for (int t = 0; t < 120; ++t)
{
// 真値: 円軌道
float angle = t * 3.0f * CV_PI / 180.0f;
float true_x = 320 + 150 * std::cos(angle);
float true_y = 240 + 150 * std::sin(angle);
// 観測値: 真値 + ノイズ
float obs_x = true_x + noise(rng);
float obs_y = true_y + noise(rng);
// 予測ステップ
cv::Mat pred = kf.predict();
// 補正ステップ
meas.at<float>(0) = obs_x;
meas.at<float>(1) = obs_y;
cv::Mat est = kf.correct(meas);
float est_x = est.at<float>(0);
float est_y = est.at<float>(1);
std::cout << t << ", "
<< true_x << ", " << true_y << ", "
<< obs_x << ", " << obs_y << ", "
<< est_x << ", " << est_y << "\n";
// 描画
canvas = cv::Scalar(30, 30, 30);
cv::circle(canvas, {(int)obs_x, (int)obs_y}, 5, {0, 100, 255}, -1); // 観測 (橙)
cv::circle(canvas, {(int)est_x, (int)est_y}, 5, {0, 255, 0}, -1); // 推定 (緑)
cv::circle(canvas, {(int)true_x, (int)true_y}, 5, {255, 255, 0}, -1); // 真値 (黄)
cv::putText(canvas, "Orange: observed Green: estimated Yellow: true",
{10, 460}, cv::FONT_HERSHEY_SIMPLEX, 0.5, {200,200,200}, 1);
cv::imshow("KalmanFilter demo", canvas);
if (cv::waitKey(30) == 27) break; // ESC で終了
}
cv::destroyAllWindows();
return 0;
}
実行結果
ウィンドウに3色の点が表示されます。
frame, true_x, true_y, obs_x, obs_y, est_x, est_y
0, 470, 240, 464.83, 252.219, 457.934, 251.637
1, 469.794, 247.85, 477.008, 256.547, 483.133, 256.658
...
橙点(観測)がジッタするのに対し、緑点(推定)は真値(黄)に追従して滑らかに動きます。フレームが進むにつれてフィルタが収束し、推定精度が向上します。
引数と戻り値
コンストラクタ
| 引数 | 型 | 説明 |
|---|---|---|
dynamParams |
int |
状態ベクトルの次元数 |
measureParams |
int |
観測ベクトルの次元数 |
controlParams |
int |
制御ベクトルの次元数(不要なら 0) |
type |
int |
行列の型(CV_32F または CV_64F) |
主なメンバ変数(行列)
| メンバ | サイズ | 意味 |
|---|---|---|
transitionMatrix |
state × state | 状態遷移行列 A |
measurementMatrix |
meas × state | 観測行列 H |
processNoiseCov |
state × state | プロセスノイズ共分散 Q |
measurementNoiseCov |
meas × meas | 観測ノイズ共分散 R |
errorCovPost |
state × state | 事後誤差共分散 P |
statePost |
state × 1 | 事後状態推定(初期値設定に使う) |
主なメソッド
| メソッド | 戻り値 | 説明 |
|---|---|---|
predict() |
cv::Mat (state×1) |
予測ステップ。事前状態推定を返す |
correct(measurement) |
cv::Mat (state×1) |
補正ステップ。観測を受け取り事後推定を返す |
実践例:検出が途切れたフレームの補間
物体検出器の出力は毎フレーム得られるとは限りません。検出ありのフレームは correct()、検出なしのフレームは predict() のみ呼ぶことで位置を補間できます。
#include <opencv2/opencv.hpp>
#include <vector>
#include <optional>
// 検出結果を模擬: 偶数フレームのみ観測が得られるシナリオ
std::optional<cv::Point2f> get_detection(int frame)
{
if (frame % 2 == 0)
return cv::Point2f(200.0f + frame * 1.5f, 240.0f);
return std::nullopt; // 奇数フレームは検出なし
}
int main()
{
cv::KalmanFilter kf(4, 2, 0, CV_32F);
kf.transitionMatrix = (cv::Mat_<float>(4, 4) <<
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1);
kf.measurementMatrix = (cv::Mat_<float>(2, 4) <<
1, 0, 0, 0,
0, 1, 0, 0);
cv::setIdentity(kf.processNoiseCov, cv::Scalar::all(1e-3));
cv::setIdentity(kf.measurementNoiseCov, cv::Scalar::all(5.0));
cv::setIdentity(kf.errorCovPost, cv::Scalar::all(1));
kf.statePost = (cv::Mat_<float>(4, 1) << 200, 240, 1.5, 0);
cv::Mat meas(2, 1, CV_32F);
cv::Mat canvas(480, 640, CV_8UC3);
for (int t = 0; t < 100; ++t)
{
// 予測は毎フレーム必ず呼ぶ
cv::Mat pred = kf.predict();
auto det = get_detection(t);
cv::Mat est;
if (det.has_value())
{
// 検出ありのフレームは correct() で補正
meas.at<float>(0) = det->x;
meas.at<float>(1) = det->y;
est = kf.correct(meas);
}
else
{
// 検出なし: 予測値をそのまま使用 (correct() は呼ばない)
est = pred;
}
float ex = est.at<float>(0);
float ey = est.at<float>(1);
canvas = cv::Scalar(30, 30, 30);
// 検出なしフレームは推定値のみ表示
if (det.has_value())
cv::circle(canvas, {(int)det->x, (int)det->y}, 7, {0, 100, 255}, -1);
cv::circle(canvas, {(int)ex, (int)ey}, 5, {0, 255, 0}, -1);
cv::imshow("Interpolation demo", canvas);
if (cv::waitKey(30) == 27) break;
}
cv::destroyAllWindows();
return 0;
}
⚠️ predict() を呼ばずに correct() だけ呼ぶ、あるいは predict() を2回連続で呼ぶと誤差共分散が正しく更新されません。1フレームにつき必ず predict → (optional) correct の順で呼ぶことが重要です。
つまずきポイント
① 行列の型不一致による assertion
コンストラクタで CV_32F を指定した場合、measurementMatrix や transitionMatrix への代入も必ず CV_32F(cv::Mat_<float>)の行列で行う必要があります。CV_64F の行列を代入したり、correct() に CV_64F の観測を渡したりすると、内部の行列積(gemm)で assertion エラーが発生します。
// NG: CV_64F の Mat を CV_32F のフィルタに渡している
kf.transitionMatrix = cv::Mat::eye(4, 4, CV_64F); // → assertionエラー
// OK
kf.transitionMatrix = cv::Mat::eye(4, 4, CV_32F);
② predict() の呼び忘れ・二重呼び出し
predict() を呼ばずに correct() を呼んでも内部的に動作はしますが、誤差共分散が一貫した状態で更新されません。逆に predict() を1フレームに2回呼ぶと誤差共分散が余分に膨らみます。ループ構造を「1フレーム = predict 1回 + correct 0〜1回」に固定してください。
③ Q と R のチューニングを間違える
processNoiseCov (Q) を大きくするとモデルより観測を信頼し、推定が観測に追従しやすくなります(ジッタが残る)。逆に measurementNoiseCov (R) を大きくするとモデル予測を信頼し、推定が滑らかになります(追従が遅れる)。チューニングはアプリの要件(追従速度 vs 平滑度)に合わせて行ってください。値の大きさは絶対値ではなく Q と R の比率が効きます。
関連する関数
- cv::VideoCapture の使い方【OpenCV/C++】〜カメラ映像を取り込む〜 — リアルタイム追跡と組み合わせる際の映像入力
- cv::VideoWriter の使い方【OpenCV/C++】〜動画ファイルを書き出す〜 — 追跡結果を動画として保存
- cv::CascadeClassifier の使い方【OpenCV/C++】〜Haar Cascadeで顔検出〜 — 検出器の出力をカルマンフィルタに渡す典型パターン
cv::BackgroundSubtractorMOG2— 背景差分で動体を抽出しカルマンフィルタと組み合わせる用途に
まとめ
cv::KalmanFilter は状態遷移行列・観測行列・ノイズ共分散を設定し、predict() → correct() を毎フレーム呼ぶだけで使えます。検出が欠けたフレームは predict() の結果で補間でき、追跡の途切れを最小限に抑えられます。Q と R の比率調整がチューニングの核心です。
🛠 画像処理のプロが開発するSDK/API
本ブログを運営するスワローインキュベートは、OpenCV ベースのなりすまし判定SDK/API(C++製・OpenCV 4.10)を開発しています。顔認証システムへの組み込み実績多数。
→ 顔認証のなりすまし対策 設計チェックリスト(当ブログ)
→ なりすまし判定SDK/APIの詳細を見る
→ API仕様書・サンプルコード

