First of all, this is the first time I try to make a Kalman filter.
I earlier posted the follwoing question Filter out noise and variations from speed values on StackOverflow which describes the background for this post. This is a typical sample of values I'm trying to filter. They don't necessarily have to decrease which is the case here. But the rate of change is typically like this
X------- Y
16 ---233.75
24 ---234.01
26 ---234.33
32 ---234.12
36 ---233.85
39 ---233.42
47 ---233.69
52 ---233.68
55 ---233.76
60 ---232.97
66 ---233.31
72 ---233.99
I have implemented my Kalman Filter according to this tutorial: Kalman Filter for Dummies.
My implementation looks like this (pseudocode).
//Standard deviation is 0.05. Used in calculation of Kalman gain
void updateAngle(double lastAngle){
if(firsTimeRunning==true)
priorEstimate = 0; //estimate is the old one here
priorErrorVariance = 1.2; //errorCovariance is the old one
else
priorEstimate = estimate; //estimate is the old one here
priorErrorVariance = errorCovariance; //errorCovariance is the old one
rawValue = lastAngle; //lastAngle is the newest Y-value recieved
kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
errorCovariance = (1 - kalmanGain) * priorErrVariance;
angle = estimate; //angle is the variable I want to update
} //which will be lastAngle next time
I start out with a prior estimate of 0. This seems to work fine. But what i notice is that the kalmanGain will decrease each time this update is run, which means that I trust my new values less the longer my filter is running (?). I don't want that.
I went from just using a moving average (simple and exponential weighted) to using this. Right now I can't even get as good results as that did.
My question is if this is the right implementation and if my prior error variance and standard deviation looks good according to the sample values I have posted? My parameters are actually just picked randomly to see if I could get some good results. I have tried several different ranges but with poor results. If you have any suggestions to changes I can do, it would be really appreciated. I'm sorry if there is some obvious things missing. First time posting here too.