位置和速度的卡尔曼滤波器：引入速度估计

感谢昨天在我的查询中发布评论/答案的每个人（为位置，速度，加速度实现Kalman过滤器）。我一直在研究推荐的内容，尤其是在（a）关于一维位置和速度的Wikipedia示例以及另一个考虑类似内容的网站上。

2013年4月26日更新：此处的原始问题包含一些错误，这与以下事实有关：我没有正确理解Wikipedia示例中的一维位置和速度。随着我对正在发生的事情有了更好的理解，我现在重新起草了这个问题，并更加集中了它。

我在上面的介绍性段落中提到的两个示例均假设仅是对位置进行了衡量。但是，两个示例都没有任何类型的速度计算$(x_k-x_{k-1})/dt$。例如，Wikipedia示例将${\bf H}$矩阵指定为，这意味着仅输入位置。以Wikipedia为例为例，卡尔曼滤波器的状态向量包含位置和速度，即X ķ X ķ ˙ X${\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 ķ ķ - 1 X ķ - 1 ˙ X ķ - 1一ķ - 1 ķ 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$$

这意味着在时间，速度的测量值\ hat {\ dot {x}} _ k由下式给出$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}$$

该方程右边的所有量（即，和）都是具有已知均值和标准偏差的正态分布随机变量，因此测量向量的矩阵$\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*}$$

可以计算。这是将速度估算引入流程的有效方法吗？

这是将速度估算引入流程的有效方法吗？

如果您适当选择状态，则速度估算是“免费”的。请参见下面的信号模型推导（对于我们一直在研究的简单一维情况）。

信号模型，取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}$$

如果我们的状态与以前一样： 则状态更新方程仅仅是： Xķ+1=（ 1个Δ 吨0 1）Xķ+（（Δ 吨）

$$\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$

这给出了与先前版本不同的矩阵，但是F和H矩阵应该相同。$\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

然后，我可以将Kalman滤波器方程式应用于此（噪声测量）。$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$$y$

然后，下面的图显示结果。

$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$