itamarwe / kalman

Kalman Filter in Javascript
113 stars 30 forks source link

Can I simply split update func to a predicate func and a update func? #6

Open navigator117 opened 6 years ago

navigator117 commented 6 years ago

like below, for predicate <--> measurement loop: KalmanModel.prototype.predicate = function()
{ //init
this.xk = this.x_k;
this.Pk = this.P_k;
//Predict
this.x_kk = this.F_k.x(this.xk);
this.P_kk = this.F_k.x(this.Pk.x(this.F_k.transpose())).add(this.Q_k);
} KalmanModel.prototype.update = function(o)
{ this.I = Matrix.I(this.P_k.rows());
this.y_k = o.z_k.subtract(o.H_k.x(this.x_kk)); //observation residual
this.S_k = o.H_k.x(this.P_kk.x(o.H_k.transpose())).add(o.R_k); //residual covariance
this.K_k = this.P_kk.x(o.H_k.transpose().x(this.S_k.inverse())); //Optimal Kalman gain
this.x_k = this.x_kk.add(this.K_k.x(this.y_k));
this.P_k = this.I.subtract(this.K_k.x(o.H_k)).x(this.P_kk);
}