The variable intial_value is initialized but is never used in the algorithm.
I believe that if x instead is initialized with the initial "best guess value", the algorithm will converge more quickly (assuming the initial guess was a good guess)
Thus
void KalmanFilter::KalmanInit(float _q, float _r, float _p, float _intial_value) {
q = _q;
r = _r;
p = _p;
x = _intial_value;
}
initial_value can be given the flick!