カルマンフィルターの実用のできるアルゴリズムのメモです。加速度センサーから取得した加速度でGPSなどで測定した位置を補正するとします。
前回の位置と加速度から今回の位置を推測します。推測値と測定値をカルマンゲインで最終値を決めます。メモは1次元でアルゴリズムですが、2次元や3次元も同じアルゴリズムで実現できます。 

以下はソースです。

#include "stdafx.h"
#include <math.h>
#include <iostream>
#include <iomanip>
#include <string>

using namespace std;

int first = 1;
double x_est = 0.0;
double x_est_last = 0.0;
double x_temp_est = 0.0;
double V_last = 0.0;    // 前回の速度
double A_last = 0.0;    // 前回の加速度
double P_temp = 0.0;
double P_last = 0.0;
double Period = 1.0;
double R = 0.5;         // Q:プロセスノイズ共分散、Qが大きいほど、動態反応速い、収束性が悪い
double Q = 0.5;         // R:測定ノイズの共分散、Rが大きいほど、動態反応遅い、収束性が良い

double KalmanFilter(double z_measured, double acceleration)
{
    double K = 0.0;
    double P = 0.0;

    /* 初期処理 */
    if (first)
    {
        first = 0;
        x_est = x_est_last = z_measured;
        A_last = acceleration;
        return x_est;
    }

    /* 加速度で位置推定 */
    x_temp_est = x_est_last + V_last * Period + A_last * Period * Period / 2.0;
    V_last += A_last * Period;
    P_temp = P_last + Q;

    /* カルマンゲイン算出 */
    K = P_temp / (P_temp + R);

    /* カルマンゲインで計測した位置と推定した位置の差の割合を決め、最終位置を算出 */
    x_est = x_temp_est + K * (z_measured - x_temp_est); 
    P = (1 - K) * P_temp;

    /* 今回の結果を記憶 */
    P_last = P;
    x_est_last = x_est;
    A_last = acceleration;

    return x_est;
}

int _tmain(int argc, _TCHAR* argv[])
{
#define NN 36
    double px[NN];
    double ax[NN];
    double vx[NN];
    // テスト用の位置と加速度のデータを作成
    for (int i = 0; i < NN; i ++) {
        px[i] = 100.0 * sin(8.0 * atan(1.0) / NN * i) + rand() % 3 - 1;
        ax[i] = 0;
        vx[i] = 0;
    }
    for (int i = 0; i < NN-1; i ++) {
        vx[i+1] = (px[i+1] - px[i]) / Period;    
    }
    for (int i = 0; i < NN - 1; i ++) {
        ax[i] = (vx[i+1] - vx[i]) / Period;    
    }

    // カルマフィルターかける
    for(int loop = 0; loop < NN; loop ++)
    {
        double px_out =  KalmanFilter(px[loop], ax[loop]);
        cout << setw(2) << loop << ", " 
             << setw(6) << fixed << setprecision(2) << px[loop] << ", "
             << setw(6) << fixed << setprecision(2) << px_out << endl;
    }
    return 0;
}

以下は実行の結果です。2列目は計測した位置であり、3列目はカルマンフィルター掛けた結果です。

 0,   1.00,   1.00
 1,  18.36,  14.02
 2,  34.20,  32.77
 3,  50.00,  49.46
 4,  65.28,  65.17
 5,  76.60,  77.32
 6,  85.60,  86.32
 7,  92.97,  93.55
 8,  98.48,  99.06
 9, 101.00, 101.79
10,  99.48, 100.55
11,  94.97,  95.95
12,  86.60,  87.71
13,  75.60,  76.53
14,  64.28,  64.70
15,  51.00,  51.53
16,  34.20,  35.08
17,  18.36,  18.52
18,  -1.00,  -0.27
19, -18.36, -18.47
20, -35.20, -35.34
21, -51.00, -51.25
22, -63.28, -64.05
23, -77.60, -77.51
24, -86.60, -87.58
25, -93.97, -94.66
26, -99.48, -100.10
27, -99.00, -100.38
28, -97.48, -98.21
29, -92.97, -93.82
30, -85.60, -86.47
31, -77.60, -78.06
32, -63.28, -64.66
33, -51.00, -51.14
34, -35.20, -35.93
35, -17.36, -18.03