Open navigator117 opened 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); }
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);
}