Bộ lọc Kalman cho vị trí và vận tốc: giới thiệu ước tính tốc độ

Cảm ơn tất cả mọi người đã đăng bình luận / câu trả lời cho truy vấn của tôi ngày hôm qua ( Thực hiện bộ lọc Kalman cho vị trí, vận tốc, gia tốc ). Tôi đã xem xét những gì được khuyến nghị, và đặc biệt ở cả (a) ví dụ wikipedia về vị trí và vận tốc một chiều và cũng là một trang web khác xem xét một điều tương tự .

Cập nhật ngày 26 tháng 4 năm 2013 : câu hỏi ban đầu ở đây có một số lỗi, liên quan đến thực tế là tôi đã không hiểu đúng về ví dụ wikipedia về vị trí và vận tốc một chiều . Với sự hiểu biết được cải thiện của tôi về những gì đang diễn ra, giờ đây tôi đã phác thảo lại câu hỏi và tập trung vào nó chặt chẽ hơn.

Cả hai ví dụ mà tôi đề cập đến trong đoạn giới thiệu ở trên đều cho rằng đó chỉ là vị trí được đo. Tuy nhiên, không ví dụ nào có bất kỳ loại tính toán nào cho tốc độ. Ví dụ: ví dụ Wikipedia chỉ định ma trận là , có nghĩa là chỉ có vị trí là đầu vào. Tập trung vào ví dụ Wikipedia, vectơ trạng thái của bộ lọc Kalman chứa vị trí và tốc độ , tức là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*}$$

Giả sử phép đo vị trí tại thời điểm là . Sau đó, nếu vị trí và tốc độ tại thời điểm là và và nếu là gia tốc không đổi áp dụng trong khoảng thời gian đến , từ đo nó có thể suy ra giá trị cho sử dụng công thứcx k k - 1 x k - 1 ˙ x k - 1 một 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$$

Điều này ngụ ý rằng tại thời điểm , phép đo của tốc độ được đưa ra bởi˙ 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}$$

Tất cả các đại lượng ở phía bên phải của phương trình đó (nghĩa là , và ) thường được phân phối các biến ngẫu nhiên với các phương tiện đã biết và độ lệch chuẩn , do đó, ma trận cho vectơ đoxk-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*}$$

có thể được tính toán Đây có phải là một cách hợp lệ để đưa các ước tính tốc độ vào quy trình?

Đây có phải là một cách hợp lệ để đưa các ước tính tốc độ vào quy trình?

Nếu bạn chọn trạng thái của mình một cách thích hợp, thì ước tính tốc độ sẽ "miễn phí". Xem đạo hàm của mô hình tín hiệu bên dưới (đối với trường hợp 1-D đơn giản mà chúng tôi đã xem xét).

Mô hình tín hiệu, mất 2

Vì vậy, chúng tôi thực sự cần phải đồng ý về một mô hình tín hiệu trước khi chúng tôi có thể tiến lên phía trước. Từ chỉnh sửa của bạn, có vẻ như mô hình vị trí của bạn, , là:$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}$$

Nếu trạng thái của chúng ta vẫn như trước: thì phương trình cập nhật trạng thái chỉ là: trong đó bây giờ của chúng tôi là gia tốc phân phối bình thường.

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

Điều đó mang lại ma trận với phiên bản trước, nhưng ma trận và phải giống nhau.$\mathbf{G}$$\mathbf{F}$$\mathbf{H}$

---

Nếu tôi thực hiện điều này trong scilab(xin lỗi, không có quyền truy cập vào MATLAB), có vẻ như:

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

Sau đó, tôi có thể áp dụng các phương trình lọc Kalman cho này (các phép đo nhiễu).$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

Vì vậy, chúng ta phải đo ồn ào của chúng tôi , và chúng tôi đã áp dụng bộ lọc Kalman với họ và sử dụng các mô hình tín hiệu tương tự để tạo ra như chúng ta làm gì để áp dụng bộ lọc Kalman (một giả định khá lớn, đôi khi!).$y$$y$

Sau đó các ô sau đây cho thấy kết quả.

Lô 1 : và so với thời gian.$y$$x_k$

Lô 2 : Chế độ xem phóng to của một vài mẫu đầu tiên:

Cốt truyện 3 : Một cái gì đó bạn không bao giờ có được trong cuộc sống thực, vị trí thực sự so với ước tính trạng thái của vị trí.

Cốt truyện 4 : Một thứ bạn cũng không bao giờ có được trong cuộc sống thực, vận tốc thực so với ước tính trạng thái của vận tốc.

Cốt truyện 5 : Định mức của ma trận hiệp phương sai trạng thái (một thứ bạn nên luôn theo dõi trong cuộc sống thực!). Lưu ý rằng nó rất nhanh chóng chuyển từ giá trị rất lớn ban đầu của nó sang một thứ rất nhỏ, vì vậy tôi chỉ hiển thị một vài mẫu đầu tiên.

Lô 6 : Sơ đồ sai số giữa vị trí và vận tốc thực và ước tính của chúng.

Nếu bạn nghiên cứu trường hợp các phép đo vị trí là chính xác, thì bạn thấy rằng các phương trình udpate Kalman tạo ra kết quả chính xác cho vị trí và tốc độ BÓNG. Về mặt toán học, thật đơn giản để xem tại sao. Sử dụng cùng một ký hiệu như bài viết trên wikipedia , các phép đo chính xác có nghĩa là . Nếu bạn giả sử rằng vị trí và tốc độ ban đầu được biết để , thì và ma trận tăng Kalman được đưa ra bởi$\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)$$

Điều này có nghĩa là quy trình cập nhật Kalman tạo ra

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

Như bạn có thể thấy, giá trị của tốc độ được đưa ra bằng chính xác công thức bạn đề xuất sử dụng cho ước tính tốc độ. Vì vậy, mặc dù bạn không thể thấy bất kỳ loại tính toán nào cho tốc độ, nhưng thực tế nó được ẩn trong đó sau tất cả.$(x_k-x_{k-1})/dt$