位置と速度のカルマンフィルター：速度推定の導入

昨日私のクエリにコメント/回答を投稿してくれたすべての人に感謝します（位置、速度、加速度のカルマンフィルターの実装）。私は、（a）の両方で推奨されていたものを見て、特にしてきた1次元位置と速度にWikipediaの例をしても同じようなことを考えて、別のウェブサイト。

アップデート2013年4月26日：ここでの元の質問には、1次元の位置と速度に関するウィキペディアの例を正しく理解していなかったという事実に関連するいくつかのエラーが含まれていました。何が起こっているのかについての理解が深まったので、質問を書き直し、より厳密に焦点を合わせました。

上記の導入部の段落で参照する例は両方とも、測定されるのは位置のみであると想定しています。ただし、どちらの例にも速度の計算がありません。たとえば、Wikipediaの例では、行列をとして指定します。これは、位置のみが入力されることを意味します。ウィキペディアの例に焦点を当て、状態ベクトルカルマンフィルタのは、位置含まと速度、すなわちH H = [ 1 0 ] X K X K ˙ X$(x_k-x_{k-1})/dt$${\bf H}$${\bf H} = [1\ \ \ 0]$${\bf x}_k$$x_k$$\dot{x}_{k}$

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$

時間での位置の測定値がます。次に、時間での位置と速度がおよびであり、が時間間隔からに適用される一定の加速度である場合、測定からはの値を推定することができます式を用いてX K K - 1 X K - 1 ˙ X K - 1 K - 1 K X$k$$\hat{x}_k$$k-1$$x_{k-1}$$\dot{x}_{k-1}$$a$$k-1$$k$$\hat{x}$$a$

$$\hat{x}_k = x_{k-1} + \dot{x}_{k-1} dt + \frac{1}{2} a dt^2$$

これは、時間で速度の測定値が次の式で与えられることを意味します。˙ X$k$$\hat{\dot{x}}_k$

$$\hat{\dot{x}}_k = \dot{x}_{k-1} + a dt = 2 \frac{\hat{x}_k - {x}_{k-1}}{dt} - \dot{x}_{k-1}$$

その方程式の右側のすべての量（つまり、、および）は、既知の平均と標準偏差を持つ正規分布のランダム変数です。 、したがって、測定ベクトルの行列XK-1 ˙ X K-1$\hat{x}_k$$x_{k-1}$$\dot{x}_{k-1}$$\bf R$

$$\begin{align*} \mathbf{\hat{x}}_{k} & =\left(\begin{array}[c]{c}\hat{x}_{k}\\ \hat{\dot{x}}_{k}\end{array} \right) \end{align*}$$

計算できます。これは、プロセスに速度推定を導入する有効な方法ですか？

これは、プロセスに速度推定を導入する有効な方法ですか？

適切に状態を選択すると、速度の見積もりは「無料」で提供されます。以下の信号モデルの導出を参照してください（これまで見てきた単純な1次元の場合）。

信号モデル、テイク2

そのため、これを前進させるには、信号モデルについて合意する必要があります。編集から、位置のモデルは次のようになります。$x_k$

$$\begin{array} xx_{k+1} &=& x_{k} + \dot{x}_{k} \Delta t + \frac{1}{2} a (\Delta t)^2\\ \dot{x}_{k+1} &=& \dot{x}_{k} + a \Delta t \end{array}$$

以前の状態の場合： 状態更新方程式は次のようになります： ここで、は正規分布の加速度です。

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$ $$\mathbf{x}_{k+1} = \left(\begin{array}[c]{c} 1\ \ \Delta t\\ 0\ \ 1\end{array} \right) \mathbf{x}_{k} + \left(\begin{array}[c]{c} \frac{(\Delta t)^2}{2} \\ \Delta t \end{array} \right) a_k$$ $a_k$

これにより、前のバージョンとは異なる行列が得られますが、および行列は同じでなければなりません。$\mathbf{G}$$\mathbf{F}$$\mathbf{H}$

---

scilab（申し訳ありませんが、matlabにアクセスできません）でこれを実装すると、次のようになります。

// Signal Model
DeltaT = 0.1;
F = [1 DeltaT; 0 1];
G = [DeltaT^2/2; DeltaT];
H = [1 0];

x0 = [0;0];
sigma_a = 0.1;

Q = sigma_a^2;
R = 0.1;

N = 1000;

a = rand(1,N,"normal")*sigma_a;

x_truth(:,1) = x0;
for t=1:N,
    x_truth(:,t+1) = F*x_truth(:,t) + G*a(t);
    y(t) = H*x_truth(:,t) + rand(1,1,"normal")*sqrt(R);
end

次に、この（ノイズのある測定値）にカルマンフィルター方程式を適用できます。$y$

// Kalman Filter
p0 = 100*eye(2,2);

xx(:,1) = x0;
pp = p0;
pp_norm(1) = norm(pp);
for t=1:N,
    [x1,p1,x,p] = kalm(y(t),xx(:,t),pp,F,G,H,Q,R);
    xx(:,t+1) = x1;
    pp = p1;
    pp_norm(t+1) = norm(pp);
end

ノイズの多い測定値があり、カルマンフィルターを適用し、カルマンフィルターを適用するのと同じ信号モデルを使用してを生成しました（かなり大きな仮定です！）。$y$$y$

次に、次のプロットが結果を示します。

プロット1：時間に対するおよび。$y$$x_k$

プロット2：最初のいくつかのサンプルの拡大図：

プロット3：実際の生活では決して得られないもの、真の位置と位置の状態推定値。

プロット4：実際の生活では決して得られないもの、真の速度と速度の状態推定値。

プロット5：状態共分散行列のノルム（実際には常に監視する必要があるもの！）最初の非常に大きな値から非常に小さな値に非常に迅速に移行することに注意してください。したがって、最初のいくつかのサンプルのみを示しました。

プロット6：真の位置と速度の間の誤差とその推定値のプロット。

位置の測定値が正確な場合を調べると、カルマン更新方程式が位置と速度の両方について正確な結果を生成することがわかります。数学的には、理由を見るのは簡単です。ウィキペディアの記事と同じ表記を使用すると、正確な測定値は意味します。あなたが初期位置と速度がそのように知られていると仮定すると、次にとカルマンゲイン行列は$\mathbf{z}_{k+1}=x_{k+1}$$\mathbf{P}_k=0$$\mathbf{P}_{k+1}^{-}=\mathbf{Q}$$\mathbf{K}_{k+1}$

$$\mathbf{K}_{k+1} = \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right)$$

これは、カルマン更新手順が生成することを意味します

$$\begin{align*} \mathbf{\hat{x}}_{k+1} & = \mathbf{F}_{k+1}\mathbf{x}_k + \mathbf{K}_{k+1}\left(\mathbf{z}_{k+1} - \mathbf{H}_{k+1} \mathbf{F}_{k+1}\mathbf{x}_k\right)\\ & = \left(\begin{array}[c]{c}x_k + \dot{x}_k dt\\ \dot{x}_k\end{array} \right) + \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right) \left(x_{k+1} - \left( x_k + \dot{x}_k dt\right) \right)\\ & = \left(\begin{array}[c]{c}x_{k+1}\\ 2 \left(x_{k+1} - x_k \right) /dt - \dot{x}_k\end{array} \right) \end{align*}$$

ご覧のとおり、速度の値は、速度の推定に使用することを提案していた式とまったく同じです。そのため、速度の計算を見ることができませんでしたが、実際にはそこに隠れています。$(x_k-x_{k-1})/dt$