Kalman filter that can filter anything and can be easily implemented! Since there was no implementation example in Java, I will write it as a memo.
public static void main(String[] args){
ArrayList<Double> result = new ArrayList<Double>();
double now = 0;
double xPre = 0;
double pPre = 0;
double sigmaW = 0;
double sigmaV = 0;
//Argument specification 1 sigmaW, 2 sigmaV
sigmaW = Integer.parseInt("5");
sigmaV = Integer.parseInt("50");
Scanner sc = new Scanner(System.in);
while(true){
now = sc.nextDouble();
if(now == -1){
break;
}
double xForecast = xPre;
double pForecast = pPre + sigmaW;
double KGain = pForecast/(pForecast + sigmaV);
double xFiltered = (xForecast + KGain*(now - xForecast));
double pFiletered = (1-KGain) * pForecast;
result.add(xFiltered);
System.out.println(xFiltered);
xPre = xFiltered;
pPre = pFiletered;
}
System.out.println("-------------------------------");
for(Double out : result){
System.out.println(out);
}
}
The degree of filtering depends on the combination of sigmaW and sigmaV.
By using this and making it into an API, you can filter various numerical values.
Although it is output as a list here, it is also possible to output the value in real time.
Recommended Posts