Triển khai bộ lọc Kalman để làm mịn dữ liệu từ deviceOrientation API

Question

Tôi đang cố gắng làm mịn dữ liệu tôi nhận được từ API thiết bịOrientation để tạo ứng dụng Google Cardboard trong trình duyệt.

Tôi đang truyền dữ liệu gia tốc kế thẳng vào vòng quay máy ảnh ThreeJs nhưng chúng tôi đang nhận được rất nhiều nhiễu trên tín hiệu khiến màn hình bị rung.

Có người đề nghị một bộ lọc Kalman là cách tốt nhất để tiếp cận làm mịn tiếng ồn xử lý tín hiệu và tôi thấy thư viện Javascript đơn giản này trên GitHub

https://github.com/itamarwe/kalman

Tuy nhiên nó thực sự thắp sáng trên tài liệu.

Tôi hiểu rằng tôi cần tạo mô hình Kalman bằng cách cung cấp Vector và 3 Ma trận làm đối số và sau đó cập nhật mô hình, một lần nữa với vectơ và ma trận làm đối số trong khung thời gian.

Tôi cũng hiểu rằng phương trình lọc Kalman có một số phần riêng biệt: vị trí ước tính hiện tại, giá trị tăng Kalman, số đọc hiện tại từ API định hướng và vị trí ước tính trước đó.

Tôi có thể thấy rằng một điểm trong không gian 3D có thể được mô tả dưới dạng Vector sao cho bất kỳ giá trị vị trí nào, chẳng hạn như vị trí ước tính hoặc đọc hiện tại có thể là Vector.

Điều tôi không hiểu là cách các phần này có thể được dịch sang Ma trận để tạo thành các đối số cho thư viện Javascript.

Answer 1

Vâng, tôi đã viết thư viện tài liệu quá đáng một vài năm trước đây. Nếu có sự quan tâm, tôi chắc chắn sẵn sàng nâng cấp nó, cải thiện tài liệu và viết các bài kiểm tra.

Hãy để tôi một thời gian ngắn giải thích tất cả các ma trận khác nhau và vectơ và làm thế nào họ nên được bắt nguồn là gì:

x - đây là vector mà bạn cố gắng để ước tính. Trong trường hợp của bạn, nó có lẽ là 3 gia tốc góc.

P - là ma trận hiệp phương sai của ước tính, có nghĩa là sự không chắc chắn của ước tính. Nó cũng được ước tính trong mỗi bước của bộ lọc Kalman cùng với x.

F - mô tả cách thức X phát triển theo mô hình. Nói chung, mô hình là x[k] = Fx[k-1]+w[k]. Trong trường hợp của bạn, F có thể là ma trận nhận diện, nếu bạn mong đợi gia tốc góc tương đối trơn tru hoặc ma trận zero, nếu bạn mong đợi khả năng tăng tốc góc hoàn toàn không thể đoán trước. Trong mọi trường hợp, w sẽ đại diện cho số tiền bạn mong đợi gia tốc thay đổi từ bước này sang bước khác.

w - mô tả tiếng ồn của quá trình, có nghĩa là, mô hình này phân kỳ ra sao từ mô hình "hoàn hảo". Nó được định nghĩa là phân phối chuẩn đa biến có nghĩa là không biến đổi với ma trận hiệp phương sai Q.

Tất cả các biến ở trên xác định mô hình của bạn, có nghĩa là những gì bạn đang cố ước tính. Trong phần tiếp theo, chúng ta nói về mô hình của quan sát - những gì bạn đo lường để ước tính mô hình của bạn.

z - đây là những gì bạn đo lường. Trong trường hợp của bạn, kể từ khi bạn đang sử dụng gia tốc kế, bạn đang đo lường những gì bạn cũng đang ước tính. Nó sẽ là sự tăng tốc góc.

H - mô tả mối quan hệ giữa mô hình của bạn và quan sát. z[k]=H[k]x[k]+v[k]. Trong trường hợp của bạn, nó là ma trận nhận dạng.

v - là tiếng ồn đo lường và được giả định là không có nghĩa là tiếng ồn trắng Gauss với hiệp phương sai R [k]. Ở đây bạn cần phải đo mức ồn là các gia tốc kế và tính toán ma trận hiệp phương sai tiếng ồn.

Nói tóm lại, các bước để sử dụng bộ lọc Kalman:

1. Xác định x[0] và P[0] - tình trạng ban đầu của mô hình của bạn, và ước tính ban đầu là như thế nào một cách chính xác bạn biết x[0].
2. Xác định F dựa trên mô hình của bạn và cách nó phát triển từ từng bước.
3. Xác định Q dựa trên bản chất ngẫu nhiên của mô hình của bạn.
4. Xác định H dựa trên mối quan hệ giữa những gì bạn đo lường và những gì bạn muốn ước tính (giữa mô hình và đo lường).
5. Xác định R dựa trên tiếng ồn đo. Làm thế nào ồn là đo lường của bạn.

Sau đó, với mỗi quan sát mới, bạn có thể cập nhật các ước lượng mô hình nhà nước sử dụng bộ lọc Kalman, và có một ước lượng tối ưu của nhà nước của mô hình (x[k]), và tính chính xác của ước lượng rằng (P[k]) .

Answer 2

var acc = { 
x:0, 
y:0, 
z:0 
}; 

var count = 0; 

if (window.DeviceOrientationEvent) { 
    window.addEventListener('deviceorientation', getDeviceRotation, false); 
}else{ 
    $(".accelerometer").html("NOT SUPPORTED") 
} 

var x_0 = $V([acc.x, acc.y, acc.z]); //vector. Initial accelerometer values 

//P prior knowledge of state 
var P_0 = $M([ 
       [1,0,0], 
       [0,1,0], 
       [0,0,1] 
      ]); //identity matrix. Initial covariance. Set to 1 
var F_k = $M([ 
       [1,0,0], 
       [0,1,0], 
       [0,0,1] 
      ]); //identity matrix. How change to model is applied. Set to 1 
var Q_k = $M([ 
       [0,0,0], 
       [0,0,0], 
       [0,0,0] 
      ]); //empty matrix. Noise in system is zero 

var KM = new KalmanModel(x_0,P_0,F_k,Q_k); 

var z_k = $V([acc.x, acc.y, acc.z]); //Updated accelerometer values 
var H_k = $M([ 
       [1,0,0], 
       [0,1,0], 
       [0,0,1] 
      ]); //identity matrix. Describes relationship between model and observation 
var R_k = $M([ 
       [2,0,0], 
       [0,2,0], 
       [0,0,2] 
      ]); //2x Scalar matrix. Describes noise from sensor. Set to 2 to begin 
var KO = new KalmanObservation(z_k,H_k,R_k); 

//each 1/10th second take new reading from accelerometer to update 
var getNewPos = window.setInterval(function(){ 

    KO.z_k = $V([acc.x, acc.y, acc.z]); //vector to be new reading from x, y, z 
    KM.update(KO); 

    $(".kalman-result").html(" x:" +KM.x_k.elements[0]+", y:" +KM.x_k.elements[1]+", z:" +KM.x_k.elements[2]); 
    $(".difference").html(" x:" +(acc.x-KM.x_k.elements[0])+", y:" +(acc.y-KM.x_k.elements[1])+", z:" +(acc.z-KM.x_k.elements[2])) 


}, 100); 

//read event data from device 
function getDeviceRotation(evt){ 

    // gamma is the left-to-right tilt in degrees, where right is positive 
    // beta is the front-to-back tilt in degrees, where front is positive 
    // alpha is the compass direction the device is facing in degrees 
    acc.x = evt.alpha; 
    acc.y = evt.beta; 
    acc.z = evt.gamma; 
    $(".accelerometer").html(" x:" +acc.x+", y:" +acc.y+", z:" +acc.z); 
} 

Đây là một trang demo cho thấy kết quả của tôi

http://cardboard-hand.herokuapp.com/kalman.html

tôi đã thiết lập tiếng ồn cảm biến để một ma trận 2 vô hướng cho bây giờ để xem nếu Kalman đang làm việc của nó nhưng chúng tôi nhận thấy cảm biến có phương sai lớn hơn trong trục x khi điện thoại nằm phẳng trên bàn. Chúng tôi nghĩ rằng đây có thể là vấn đề với khóa Gimbal. Chúng tôi chưa thử nghiệm nhưng có thể thay đổi phương sai trong mỗi trục tùy thuộc vào hướng của thiết bị.