1 Dimensional Kalman Filter

kalman1d.cpp

void filter(VectorXd &x, MatrixXd &P) {

	for (unsigned int n = 0; n < measurements.size(); ++n) {

		VectorXd z = measurements[n];
		//YOUR CODE HERE
		
		// KF Prediction
		VectorXd x_ = F*x + u;
		MatrixXd P_ = F*P*F.transpose() + Q;
		
		// KF Measurement step
		VectorXd y = z - H*x_;
		MatrixXd S = H*P_*H.transpose() + R;
		MatrixXd K = P_ * H.transpose() * S.inverse();
		 
		// new state
		x = x_ + K*y;
		P = (I-K*H)*P_;
		
		std::cout << "x=" << std::endl <<  x << std::endl;
		std::cout << "P=" << std::endl <<  P << std::endl;


	}
}