カルマンフィルターの簡単な紹介
カルマン フィルターは、不確実な要素を含む動的システム状態を推定するために使用される最適化アルゴリズムです。元々は Rudolf E. カルマンが提案したもので開発されました。 1960年代に。このアルゴリズムは、さまざまな工学および科学分野、特に制御システム、ナビゲーション、自動運転、信号処理などで広く使用されています。
カルマン フィルターは、確率論的推論に基づく手法です。システムの予測モデルと測定データを融合することでシステムの状態を推定します。特にノイズのある動的システムに適しています。各タイム ステップで、カルマン フィルタリングは 2 つの主要なステップを実行します。
- 予測ステップ (予測フェーズ): システムの動的モデルと以前の状態の推定に基づいて、現時点の状態を予測します。この予測では、システムの物理法則と外部入力が考慮されます。
- 更新ステップ (更新ステージ): 測定データを受信した後、カルマン フィルターは予測された状態と実際の測定値を組み合わせて、加重平均を通じて状態推定を修正します。これにより、より正確な状態推定値が得られる。カルマン フィルターは、測定データの信頼性に基づいて重みを自動的に調整します。その影響は、データの信頼性が高いほど大きくなります。
車を運転しているところを想像してください。現在の速度を示す速度計がありますが、それは揺れていたり不正確である可能性があります。同時に、目標に向かって運転しているときなど、道路上で実際に何が起こっているかを見る目も持っています。現在、カルマン フィルターはスマート ドライバーのように機能し、速度計の推定値と実際に目で見ているものを組み合わせて、速度推定値を賢明な方法で修正します。こうすることで、運転速度がより正確になります。もちろん、カルマンフィルターの役割は車速の推定だけにとどまらず、現代の科学や工学の分野でも決定的な役割を果たしています。 カルマン フィルタリングはすべて、ナビゲーション システムにおける航空機の位置と姿勢の推定、レーダー追跡ターゲット、ビデオ ターゲット追跡、信号ノイズ除去、およびオブジェクトの位置決めに役割を果たします。ガールフレンドの最適な体重推定値をリアルタイムで取得することもできます (持っている場合)。
予測式を導入する
一次元空間を加速度 f t m \frac{f_t}{m} で前進している車があります。メートルft 、時刻 t における瞬間速度は v t v_t でt 、時刻 t における車の位置は x t x_t バツt 。中学生の知識によると、車の位置式は次のとおりです。
x t = x t − 1 + v t ⋅ △ t + 1 2 f t − 1 m △ t 2 x_t = x_{ t-1} + v_t· \triangle t + \frac{1}{2}\frac{f_{t-1}}{m}\triangle t^2 バツt =バツt−1 +でt ⋅△t+21 メートルft−1 △t2
車の速度の公式は次のとおりです。
v t = v t − 1 + f t − 1 m △ t v_t = v_{ t-1} + \frac{f_{t-1}}{m}\triangle t でt =でt−1 +メートルft−1 △t
車の状態が X t = [ x t , v t ] T X_t=[x_t,v_t]^T であると仮定します。バツt =[xt 、でt 】T は、上記の 2 つの式が次の行列で表現できることを意味します。
X t = [ x t v t ] = [ 1 △ t 0 1 ] ∗ [ x t − 1 v t − 1 ] + [ 1 2 △ t 2 △ t ] ⋅ f t − 1 m X_t = \begin{bmatrix} x_{t} \\ v_{t}\end{bmatrix}= \begin {bmatrix} 1 & \triangle t \\ 0 & 1 \end{bmatrix} * \begin{bmatrix} x_{t-1} \\ v_{t-1} \end{bmatrix} + \begin{bmatrix } \frac {1}{2}\triangle t^2 \\ \triangle t \end{bmatrix}·\frac{f_{t-1}}{m} バツt =[バツt でt 】=[10 △t1 】∗[バツt−1 でt−1 】+[21 △t2△t 】⋅メートルft−1
假设 F = [ 1 △ t 0 1 ] 、 B = [ 1 2 △ t 2 △ t ] 、 U t = f t − 1 m 假设 F = \begin{bmatrix} 1 & \三角t \\ 0 & 1 \end{bmatrix}, B=\begin{bmatrix} \frac{1}{2}\triangle t^2 \\ \triangle t \end{bmatrix} ,U_t = \frac{f_{t-1}} {男}假设F=[10 △t1 】、B=[21 △t2△t 】、でt =メートルft−1
上記の行列式は次のように変換されます。
X t = F ∗ X t − 1 + B U t − 1 1} バツt =F∗バツt−1 +BUt−1
こちら F F F は状態遷移行列です。ただし、この予測式には誤差（路面の凹凸など）の影響が避けられないため、現時点では X t X_t バツt は最良の推定値ではないため、 X ^ − \hat{X}^- に変更されます。バツ^−。ここで、- 記号は事前の値を表し、動的な測定データが含まれていないため、正確ではありません。
これによれば、カルマン フィルタリングの 5 つの核となる公式を導入できます。
カルマン フィルターの 5 つの核となる公式
预测部分：
X ^ t − = F X ^ t − 1 + B U t − 1 \hat{X}^-_t = F \hat{X}_{t -1} + BU_{t-1} バツ^t− =Fバツ^t−1 +BUt−1
P t − = F P t − 1 F T + Q P_t^- = FP_{t-1}F^T+QPt− =ファパt−1 FT+Q
更新部分：
K t = P t − H T ( H P t − H T + R ) − 1 K_t = P^-_tH ^T(HP^-_tH^T+R)^{-1} Kt =Pt− HT(HPt− HT+R)−1
X ^ = X t − + K t ( Z t − H X ^ t − ) \hat{X} = X ^-_t+K_t(Z_t-H\hat{X}_t^-) バツ^=バツt− +Kt (Zt −Hバツ^t− )
P T = ( I − K t H ) P t − P_T=(I-K_tH)P^-_t PT =(私−Kt H)Pt−
各パラメータの意味を説明します——
X ^ t − \hat{X}^-_tバツ^t− : 時間 t における事前に予測された状態は正確ではありません。
P T − P_T^-PT− :予測誤差の共分散行列は、時刻 t におけるシステムの不確実性を表します。
F FF: 状態遷移行列。一部のシステムは、温度測定など、車の動作のように F を要約できません (MATLAB 実装を参照)
うぅぅぅ_ぅでt : 時刻 t におけるシステムへの既知の外部影響
QQQ: プロセス ノイズの共分散行列
R RR: 観測ノイズの共分散行列
KtK_tKt : カルマン ゲイン。予測された状態の共分散 P と観測された状態の共分散 R のサイズを重み付けして、2 つの重要性を決定します。予測モデルをより信頼する場合は、 Kt K_t Kt 観測モデルをより信じると、 は小さくなり、 K t K_t Kt 大きくなる。さらに、カルマン ゲインにはすべての予測変数が含まれるため、更新時にすべての変数を同時に更新できます。
ZtZ_tとt : 実際の観測値は次の式になります: Z t = H X t + V Z_t=HX_t+V とt =HXt +V，其中 H H H是观测矩阵， V V V は観測ノイズです。 この式はマルチセンサー フュージョンの中核となる式であり、観測方程式とも呼ばれます。位置のみを観察する場合は、 H = [ 1 , 0 ] H=[1, 0] H=[1,0]，得到的 Z t Z_t とt 数値です。
反復計算式とその中に負の上付き文字が含まれる記号に必ず注意してください。
matlab 実装 1: 車の運動軌跡
%卡尔曼滤波估计小车的运动，这是一个含有不确定性噪声的动态系统，需要做最优估计
clc;
clear;
Z = (1:100); %假设小车理想情况下是匀速运动，每隔1s观测一次。后面会加上噪声
noise = randn(1,100);
Z = Z + 20*noise; %观测系统的小车的状态，可以适当突出噪声演示kalman的作用
X = [0; 0]; %预测的初始值，设为多少不重要
P = [1 0; 0 1]; %预测系统的初始协方差，设为多少不重要
F = [1 1; 0 1]; %状态转移矩阵，由于1s观测一次，delta_t为1
Q = [0.0001 0; 0 0.0001]; %相信观测转移矩阵，误差设置的较小
H = [1 0]; %观测矩阵，只观测到小车位置
R = 1; %由于noise设置为标准正态分布，这里观测的协方差设置为1
X_data = zeros(500, 2); %存储小车最优状态估计，初始设置为0，后面会不断更新，500行数据需要迭代500次完成更新
for t=1:500
X_ = F * X;
P_ = F * P * F' + Q;
K = P_ * H' / (H * P_ * H' + R);
X = X_ + K * (Z(t) - H * X_);
X_data(t, 1) = X(1);
X_data(t, 2) = X(2);
P = (eye(2) - K * H) * P_;
end
plot(X_data(:, 1), 'b-');
xlabel('时间/s')
ylabel('小车位置/m')
title('kalman filter');
hold on;
plot(Z, 'k+');
legend('kalman最优估计值', '带噪声的观测值');
hold off;
カルマンは動的な最適推定アルゴリズムであるため、最終的に最適値を出力するのではなく、観測を追加するたびに事後推定を実行します。これは観測用にプリントアウトできます。
自動車の位置の推定に関しては、最適な推定値が分布の中心にあることがわかり、これは予想どおりです。
matlab 実装 2: 最適温度推定
clc;
clear;
Z = 24 + randn(1, 4000); %假设测量值是24加上高斯噪声N(0,1)
X = zeros(1, 4000); %最优估的初始值，后面会迭代更新
P = zeros(1, 4000); %预测模型的协方差，注意这里并不是一个对称矩阵
Q = 0.001; %相信观测转移矩阵，误差设置的较小
R = 1; %观测模型的协方差
for t=2:4000
X_(t) = X(t-1);
P_(t) = P(t-1) + Q;
K(t) = P_(t) / (P(t) + R);
X(t) = X_(t) + K(t) * (Z(t) - X_(t));
P(t) = (1 - K(t)) * P_(t);
end
figure;
plot(Z, 'k+');
hold on;
plot(X, 'b-', 'linewidth', 1);
hold on;
plot(24*ones(1, 4000), 'r-', 'linewidth', 1);
画像の一部を拡大してみましょう。
最適な推定値は真の値に近いことがわかります (ある化学試薬の真の温度が 24 であると仮定しますが、液体中に温度計を置いて直接測定することはできません。測定できると仮定します)。液体の上では周囲の温度が乱れます（いわゆる観測値にはノイズが含まれており、最適な温度推定値を得るにはこのノイズを可能な限り除去する必要があります）。
カルマン フィルタリングが線形推定しか実行できない場合、その重要性は大幅に低下します。カルマン フィルタリングのより重要なポイントはデータの融合です。 ！ ！ 。ただし、最も古典的なカルマン フィルターはデータ フュージョンのバックグラウンド ボードとしてのみ使用でき、現在では、KEF、UEF など、データ フュージョンで優れたパフォーマンスを発揮するさまざまなカルマン バリアントが存在します。
上記の意見が改善できる場合は修正してください。整理するのは簡単ではありません。気に入ったら、「いいね！」をお願いします。