Bộ lọc Kalman - Thực hiện, Thông số và Điều chỉnh

Trước hết, đây là lần đầu tiên tôi thử tạo bộ lọc Kalman.

Tôi trước đó đã đăng câu hỏi theo dõi Lọc lọc tiếng ồn và các biến thể từ các giá trị tốc độ trên StackOverflow mô tả nền cho bài đăng này. Đây là một mẫu điển hình của các giá trị tôi đang cố gắng lọc. Họ không nhất thiết phải giảm đó là trường hợp ở đây. Nhưng tốc độ thay đổi thường như thế này

X ------- Y
16 --- 233,75
24 --- 234,01
26 --- 234,33
32 --- 234,12
36 --- 233,85
39 --- 233,42
47 --- 233,69
52 --- 233,68
55 --- 233,76
60 --- 232,97
66 --- 233,31
72 --- 233,99

Tôi đã triển khai Bộ lọc Kalman của mình theo hướng dẫn này: Bộ lọc Kalman cho Người giả .

Việc thực hiện của tôi trông như thế này (mã giả).

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

Tôi bắt đầu với ước tính trước là 0. Điều này dường như hoạt động tốt. Nhưng điều tôi nhận thấy là kalmanGain sẽ giảm mỗi khi bản cập nhật này được chạy, điều đó có nghĩa là tôi tin tưởng các giá trị mới của mình ít hơn khi bộ lọc của tôi chạy lâu hơn (?). Tôi không muốn điều đó.

Tôi đã đi từ chỉ sử dụng một trung bình di chuyển (trọng số đơn giản và theo cấp số nhân) để sử dụng này. Ngay bây giờ tôi thậm chí không thể có được kết quả tốt như đã làm.

Câu hỏi của tôi là nếu đây là triển khai đúng và nếu phương sai lỗi trước và độ lệch chuẩn của tôi có vẻ tốt theo các giá trị mẫu tôi đã đăng không? Các thông số của tôi thực sự chỉ được chọn ngẫu nhiên để xem liệu tôi có thể nhận được một số kết quả tốt hay không. Tôi đã thử một vài phạm vi khác nhau nhưng kết quả kém. Nếu bạn có bất kỳ đề xuất nào về những thay đổi tôi có thể làm, nó sẽ thực sự được đánh giá cao. Tôi xin lỗi nếu thiếu một số thứ rõ ràng. Lần đầu tiên đăng ở đây quá.

Bộ lọc Kalman rất hữu ích khi tín hiệu đầu vào của bạn bao gồm các quan sát nhiễu về trạng thái của hệ thống động lực tuyến tính. Đưa ra một loạt các quan sát về trạng thái hệ thống, bộ lọc Kalman nhằm mục đích cung cấp đệ quy các ước tính tốt hơn và tốt hơn về trạng thái của hệ thống cơ bản. Để áp dụng thành công, bạn cần có một mô hình cho tính năng động của hệ thống có trạng thái bạn đang ước tính. Như được mô tả chi tiết tại Wikipedia , mô hình này mô tả cách hệ thống cơ bản dự kiến ​​sẽ thay đổi theo một bước thời gian, với trạng thái trước đó, bất kỳ đầu vào nào của hệ thống và thành phần ngẫu nhiên phân tán Gaussian gọi là nhiễu quá trình.

Như đã nói, không rõ câu hỏi của bạn cho dù bạn có bất kỳ mô hình cơ bản như vậy. Bài đăng được liên kết chỉ ra rằng bạn đang tiêu thụ giá trị tốc độ từ một cảm biến. Chúng có thể được mô hình hóa như các quan sát trực tiếp về trạng thái của hệ thống (trong đó trạng thái là tốc độ của nó) hoặc các quan sát gián tiếp về trạng thái của nó (ví dụ như trạng thái là vị trí của nó). Nhưng, để sử dụng khung Kalman, bạn cần chọn một mô hình cho việc trạng thái đó được dự kiến ​​sẽ phát triển như thế nào khi thời gian trôi qua; thông tin bổ sung này được sử dụng để tạo ước tính tối ưu. Bộ lọc Kalman không phải là hộp đen ma thuật sẽ "dọn sạch" tín hiệu được áp dụng cho nó.

Như đã nói, hiện tượng mà bạn đã ám chỉ, nơi bộ lọc Kalman sẽ ngày càng tự tin vào đầu ra của chính nó đến mức các quan sát đầu vào dần dần bị bỏ qua, xảy ra trong thực tế. Điều này có thể được giảm thiểu bằng cách tăng thủ công các giá trị trong ma trận hiệp phương sai nhiễu quá trình. Sau đó, về mặt định tính, mô hình chuyển đổi trạng thái của hệ thống chứa thành phần ngẫu nhiên lớn hơn, do đó khả năng người ước tính dự đoán chính xác trạng thái tiếp theo với trạng thái hiện tại bị giảm đi. Điều này sẽ làm giảm sự phụ thuộc vào ước tính hiện tại của nó về trạng thái hệ thống và tăng sự phụ thuộc vào các quan sát tiếp theo, ngăn chặn hiệu ứng "bỏ qua đầu vào".

Nếu tôi hiểu chính xác, bạn có thứ gì đó đang di chuyển và bạn có thể quan sát tốc độ và tốc độ này là ồn ào. Từ các phép đo của bạn, bạn quan sát 2 loại biến thể. \

1. Biến thể gây ra bởi tiếng ồn
2. Biến thể vì đối tượng thực sự thay đổi tốc độ (ví dụ: quay)

Lý do mức tăng Kalman của bạn bằng không là vì bạn đã ngầm giả định rằng tốc độ của vật thể là không đổi và tất cả những gì bạn cần làm là ước tính tốc độ thực sự này.

" Này, tôi có một vật đang chuyển động với tốc độ không đổi và tôi muốn ước tính tốc độ không đổi này "

$x_k$$k$$y_k$

$$x_k = x_{k-1}$$ $$y_k = x_k + q_k$$

Nhưng đối tượng của bạn không di chuyển theo cách đó. Tốc độ đang thay đổi và bạn không biết nó sẽ thay đổi như thế nào và khi nào.

Thay vào đó, những gì bạn phải nói là:

" Này, tôi có một vật thể đang di chuyển với tốc độ nhưng tôi không chắc về cách nó thay đổi tốc độ của nó "

Có nhiều cách bạn có thể làm điều này: Cách đơn giản nhất là thêm sự không chắc chắn về trạng thái của bạn.

$$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$$ $$y_k = x_k + q_k$$ $q_k$$v_k$

Phương trình bộ lọc Kalman của bạn sẽ trông như thế này:

$$\hat y_{k|k-1} = \hat x_{k|k-1}$$ $$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$$ $$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$$ $$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$$ $$P_{k+1|k} =P_{k|k} + Q^s$$

0.05$Q^o$$Q^s$

Trong mã của bạn, sửa đổi nhỏ sẽ là:

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

stateVariance$Q^s$

stateVarianceGiá trị này có thể là bất cứ điều gì bạn muốn. Nó dựa trên sự tự tin của bạn về tốc độ thực sự sẽ thay đổi. Nếu bạn nghĩ tốc độ sẽ duy trì khá ổn định, hãy đặt tốc độ này thành một số nhỏ.

Bằng cách này, Kalman Gain của bạn sẽ không về không.

Bạn cần một hệ thống động để sử dụng Bộ lọc Kalman.

Tôi sẽ đề nghị

$$y = \sum\limits_{i =0}^n a_i\, x^i$$

$$a[k+1] = a[k] + w$$ $$cov(w) = Q$$ $$z = \sum\limits_{i =0}^n a_i\, x^i = y$$

$x$$a$

Tôi nghĩ rằng bạn có thể sử dụng một số ý tưởng từ lý thuyết điều khiển cổ điển, ví dụ bộ điều khiển PID .

Tín hiệu Y của bạn có thể là điểm đặt của bộ điều khiển u (t). Nhà máy quá trình chỉ là 1 và y (t) sẽ được lọc đầu ra. Tất cả bạn sẽ phải làm là đặt các tham số (điều chỉnh) P, I và D để có được những gì bạn muốn.

Đầu ra y (t) sẽ cố gắng "theo dõi" đầu vào u (t), nhưng các tham số kiểm soát việc theo dõi này sẽ như thế nào.

Hệ số khuếch đại D sẽ làm cho phản ứng của bạn nhạy cảm với những thay đổi lỗi nhanh chóng. Trong trường hợp của bạn, tôi nghĩ D nên nhỏ. Bạn không muốn y (t) thay đổi nếu u (t) thay đổi đột ngột.

Hệ số khuếch đại 'Tôi' sẽ làm cho phản hồi của bạn nhạy cảm với lỗi tích lũy. Bạn nên đặt một giá trị cao ở đó. Nếu u (t) thay đổi cấp độ và giữ nó ở đó, lỗi sẽ tăng lên và sau đó bạn muốn y (t) làm điều tương tự.

Việc tăng P có thể cho một tinh chỉnh. Dù sao, hãy thử chơi với các tham số và xem những gì bạn nhận được.

Mặc dù có nhiều phương pháp điều chỉnh phức tạp, nhưng tôi không tin rằng bạn sẽ cần nó.

Chúc may mắn.

Dưới đây là cách triển khai Bộ lọc Kalman đơn giản và rõ ràng theo các ký hiệu như trong trang Wikipedia. https://github.com/zziz/kalman-filter