Java卡尔曼滤波算法

卡尔曼滤波

在一些监测场合有一定的作用，比如温湿度，倾角，压强等
但不能单纯的靠这个，滤波出来的算法最好当做辅助用

package com.geom.kalman;

public class Kalman
{
	public float filterValue;//滤波后的值
	public float kalmanGain;//Kalamn增益
	public float A;//状态矩阵
	public float H;//观测矩阵
	public float Q;//状态矩阵的方差
	public float R;//观测矩阵的方差
	public float P;//预测误差
	public float B;
	public float u;
	
	public Kalman()
	{
		this.A = 1;
		this.H = 1;
		this.P = 0.1f;
		this.Q = 0.05f;
		this.R = 0.1f;
		this.B = 0.1f;
		this.u = 0;
		this.filterValue = 0;
	}
	
	public float filter( float newValue )
	{
		float predictValue = this.A*this.filterValue+this.B*this.u;//计算预测值
	    this.P = this.A*this.A*this.P + this.Q;//求协方差
	    this.kalmanGain = this.P * this.H /(this.P * this.H * this.H + this.R);//计算卡尔曼增益
	    this.filterValue = predictValue + (newValue - predictValue)*this.kalmanGain;//计算输出的值
	    this.P = (1 - this.kalmanGain* this.H)*this.P;//更新协方差
	    return this.filterValue;
	}
}

测试代码

public static void main(String[] args)
	{
		float arr[] = new float[] {52.768990f,52.727623f,52.733238f,52.783291f,52.717407f,52.742943f,52.744987f,52.702087f,52.759285f,52.806786f,52.808826f,52.804230f,52.877266f,52.902802f,52.939064f,52.954895f,52.949280f,52.963066f,52.867050f};
		float arrN[] = new float[arr.length];
		
		Kalman kl = new Kalman();
		kl.filterValue = arr[0];
		for( int i=0; i<arr.length; i++ )
		{
			arrN[i] = kl.filter(arr[i]);
		}
		
		float t = 0;
		for( int i=0; i<arr.length; i++ )
		{
			t += arr[i];
			System.out.print(arr[i] + ", ");
		}
		
		System.out.println();
		System.out.println("total: " + t/arr.length);
		
		t = 0;
		for( int i=0; i<arr.length; i++ )
		{
			t += arrN[i];
			System.out.print(arrN[i] + ", ");
		}
		
		System.out.println();
		System.out.println("total: " + t/arr.length);
		
		System.out.println();
	}