위치 및 속도에 대한 칼만 필터 : 속도 추정 소개

어제 내 쿼리에 의견 / 답변을 게시 한 모든 사람들에게 감사드립니다 ( 위치, 속도, 가속도에 대한 칼만 필터 구현 ). 내가 추천 한 것을보고, 그리고 봤는데 모두의 (a)에서 특히 하나 개의 차원 위치와 속도에 위키 피 디아 예 와도 비슷한 일을 고려 다른 웹 사이트 .

2013 년 4 월 26 일 업데이트 : 여기의 원래 질문에는 1 차원 위치 및 속도에 대한 Wikipedia 예제를 올바르게 이해하지 못했다는 사실과 관련된 몇 가지 오류가 포함되어 있습니다 . 진행 상황에 대한 이해가 향상됨에 따라 이제 질문을 다시 작성하고 더 엄격하게 집중했습니다.

위의 소개 단락에서 언급 한 두 가지 예는 측정 된 위치 만 가정합니다. 그러나 어느 예제도 속도에 대한 계산 가 없습니다. 예를 들어, Wikipedia 예제는 행렬을 . 이는 위치 만 입력됨을 의미합니다. Wikipedia 예 를 중심으로 Kalman 필터 의 상태 벡터 는 위치 및 속도 .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 $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 가 다음과 같이 주어진다는 것을 의미합니다˙ 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}$$

해당 방정식의 오른쪽에있는 모든 수량 (예 : $\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*}$$

계산할 수 있습니다. 속도 추정값을 프로세스에 도입 할 수있는 올바른 방법입니까?

속도 추정값을 프로세스에 도입 할 수있는 올바른 방법입니까?

주를 적절히 선택하면 속도 추정치가 "무료"로 나타납니다. 아래 신호 모델 도출을 참조하십시오 (단지 살펴본 간단한 1D 사례).

신호 모델, 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}$$

상태가 이전과 같다면 : 그러면 상태 업데이트 방정식은 다음과 같습니다. 여기서 는 정규 분포 가속도입니다. xk+1=( 1 Δ t 0 1 )xk+( ( Δ t )

$$\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 : 당신은 또한 실제 생활에서 절대 얻을 수없는 것, 실제 속도 대 속도의 상태 추정치.

Plot 5 : 상태 공분산 행렬의 규범 (실제로 항상 모니터링해야하는 것!) 초기 매우 큰 값에서 매우 작은 값으로 매우 빠르게 이동하므로 처음 몇 개의 샘플 만 표시했습니다.

플롯 6 : 실제 위치와 속도 및 추정치 사이의 오차를 플롯합니다.

위치 측정이 정확한 경우를 연구하면 Kalman udpate 방정식에서 BOTH 위치 및 속도에 대한 정확한 결과를 얻을 수 있습니다. 수학적으로 이유를 보는 것이 간단합니다. wikipedia article 과 동일한 표기법을 사용하면 정확한 측정은 입니다. 사용자는 초기 위치 및 속도가 너무 공지되어 있다고 가정하면 , 다음 및 칼만 이득 행렬 은$\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)$$

이것은 Kalman 업데이트 절차가

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