Kalman Filter卡尔曼滤波 java实现

tiger_angel

已于 2022-08-09 20:08:57 修改

阅读量2.7k

点赞数 1

分类专栏：地理空间文章标签：学习卡尔曼 kalman KalmanFilter

于 2022-08-09 19:58:44 首次发布

本文链接：https://blog.csdn.net/qq_38567039/article/details/126243605

版权

地理空间专栏收录该内容

17 篇文章 4 订阅

订阅专栏

我的理解

重点-理解1：卡尔曼滤波就是-测量值与预测值之间取最优结果-得到最优结果

重点-理解2：卡尔曼滤波就是-上一次最优结果预测当前的值，同时使用观测者修正当前值，得到最优结果

列子：再汽车行驶途中，如何确认定位自己所在位置？

提供的数据：1.已知加速度信息，2里程表信息，3GPS信息。三种数据都存在误差

说明：卡尔曼滤波结合已知信息，估计最优位置，本质是优化估计算法，例如估计人在下一帧的位置。说白了综合已知数据取最优值，阿波罗登月这哥们也用上了计算轨迹。

状态向量:(位置和速递) $\huge x_{t}= ^{}\begin{bmatrix} x_{t} \\ \dot{x_{t}} \end{bmatrix}$

$x_{t}$ 位置

$\dot{x_{t}}$ 速度

$x_{t}$ -1 上一秒的位置

$\dot{x_{t}}$ -1 上一秒的速度

$\Delta t$ 时间

$u_{t}$ (这儿小车的速度是一样的)小车加速度

$f_{t}$

$m$

$\frac{1}{2}\frac{^{f_{t}}}{m}\Delta t^{2}$ 小车上一秒和下一秒的加速度

$\frac{^{f_{t}}}{m}\Delta t$ 加速度乘以时间

第一种:

package com.zz.meridian.KalmanFilter;


import java.util.ArrayList;

/**
 * 重点：测量值与预测值之间取最优结果-让算法去算取折中的最优值
 *
 */
public class KalmanFilter {

    /**Kalman Filter*/
    private Integer predict; //观察数据值
    private Integer current; //观察数据值的下一条数据
    private Integer estimate;//每一次计算出的-最终估计值
    private double pdelt; //系统测量误差-为计算高斯噪声方差
    private double mdelt; //系统测量误差-为计算高斯噪声方差
    private double Gauss; //高斯噪声方差
    private double kalmanGain;//估计方差
    //信任程度 因为实际中不同传感器精度不同昂贵的高精度传感器就可以更信任一些Ｒ可以小一些。  或者我建立的模型很优秀误差极小就可以更信任模型Ｑ可以小一些
    /*
    QR：
        Q模型误差与R测量误差的大小，是模型预测值与测量值的加权
            R固定，Q越大，代表越信任侧量值，
            Q无穷代表只用测量值；反之，
            Q越小代表越信任模型预测值，Q为零则是只用模型预测
        Q是系统过程噪声的协方差矩阵，而R则是观测噪声的协方差矩阵。后者和你选择的传感器息息相关,R是看传感器精度，Q是过程误差看环境的影响大不大，我一般Q取0.01
        R为大于0常数都可以 比如1. P初始值设为足够大的对角矩阵。Q大小影响收敛速度。可以试验几个数值。
        Q和R分别代表对预测值和测量值的置信度（反比），通过影响卡尔曼增益K的值，影响预测值和测量值的权重。越大的R代表越不相信测量值。
        q越小，越依赖系统模型，r越小，越依赖观测值
     */
    private final static double Q = 0.00001; //（自定义-调参用）
    private final static double R = 0.1; //（自定义-调参用

    public void initial(){
//        pdelt = 4;    //系统测量误差
//        mdelt = 3;
        pdelt = 4;   //系统测量误差
        mdelt = 3;  //估计方差
    }
    public Integer KalmanFilter(Integer oldValue,Integer value){
        //(1)第一个估计值
        predict = oldValue;
        //第二个估计值
        current = value;
        //(2)高斯噪声方差
        Gauss = Math.sqrt(pdelt * pdelt + mdelt * mdelt) + Q;
        //(3)估计方差
        kalmanGain = Math.sqrt((Gauss * Gauss)/(Gauss * Gauss + pdelt * pdelt)) + R;
        //(4)最终估计值
        estimate = (int) (kalmanGain * (current - predict) + predict);
        //(5)新的估计方差，下一次不确定性的度量
        mdelt = Math.sqrt((1-kalmanGain) * Gauss * Gauss);

        return estimate;
    }

    public static void main(String[] args) {
        KalmanFilter kalmanfilter =new KalmanFilter();
        kalmanfilter.initial();
        ArrayList<Integer> list = new ArrayList<Integer>();
        list.add(-75);
        list.add(-76);
        list.add(-81);
        list.add(-75);
        list.add(-77);
        list.add(-76);
        list.add(-86);

        int oldvalue = list.get(0);
        ArrayList<Integer> alist = new ArrayList<Integer>();
        for(int i = 0; i < list.size(); i++){
            int value = list.get(i);
            oldvalue = kalmanfilter.KalmanFilter(oldvalue,value);
            alist.add(oldvalue);
        }

        System.out.println(list);
        System.out.println(alist);

    }

}

第二种:

package com.zz.meridian.KalmanFilter3;

/**
 * 卡尔曼滤波
 */

import java.util.ArrayList;

/*
关于卡尔曼滤波器的原理，网上有很多，这里就不做过多介绍，此demo为一阶卡尔曼滤波器的实现。
主要为五个公式 （后面为一阶滤波器的系数）
X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k), A=1,BU(k) = 0
P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1
Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1
X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1
P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1
 */
public class KalmanFilter {

    private final double Q = 0.000001;
    private final double R = 0.001;
    private ArrayList<Double> dataArrayList;
    private int length;

    private double z[]; // data
    private double xhat[];
    private double xhatminus[];
    private double P[];
    private double Pminus[];
    private double K[];

    public KalmanFilter(ArrayList<Double> arrayList) {
        this.dataArrayList = arrayList;
        this.length = arrayList.size();
        z = new double[length];
        xhat = new double[length];
        xhatminus = new double[length];
        P = new double[length];
        Pminus = new double[length];
        K = new double[length];
        xhat[0] = 0;
        P[0] = 1.0;

        for (int i = 0; i < length; i++) {
            z[i] = (double) dataArrayList.get(i);
        }
    }

    public ArrayList<Double> calc() {
        if (dataArrayList.size() < 2) {
            return dataArrayList;
        }
        for (int k = 1; k < length; k++) {
            // X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k),A=1,BU(k) = 0
            xhatminus[k] = xhat[k - 1];

            // P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1
            Pminus[k] = P[k - 1] + Q;

            // Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1
            K[k] = Pminus[k] / (Pminus[k] + R);

            // X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1
            xhat[k] = xhatminus[k] + K[k] * (z[k] - xhatminus[k]);

            //P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1
            P[k] = (1 - K[k]) * Pminus[k];
        }

        for (int i = 0; i < length; i++) {
            dataArrayList.set(i, xhat[i]);
        }
        dataArrayList.remove(0);
        return dataArrayList;
    }

    public static void main(String[] args) {
        ArrayList<Double> list = new ArrayList<>();
        list.add(-75.);
        list.add(-76.);
        list.add(-81.);
        list.add(-75.);
        list.add(-77.);
        list.add(-76.);
        list.add(-86.);
        System.err.println("滤波前的:"+list);
        KalmanFilter kalmanFilter = new KalmanFilter(list);
        kalmanFilter.calc();
        System.err.println("滤波后的:"+kalmanFilter.dataArrayList);
    }
}

第三种JKalman代码:

工具代码下载:JKalman download | SourceForge.net

JKalman官方没得说明,见我最后写的main测试,即可看懂如何时候用

外网不行,下载这个

KalmanFilter卡尔曼滤波java实现-Java文档类资源-CSDN下载

官方JKalman测试类

/*******************************************************************************
 *
 *  JKalman - KALMAN FILTER (Java) TestBench
 *
 *  Copyright (C) 2007 Petr Chmelar
 *
 *  By downloading, copying, installing or using the software you agree to
 *  the license in licenseIntel.txt or in licenseGNU.txt
 *
 **************************************************************************** */

package com.zz.meridian.KalmanFilter5;

import com.zz.meridian.KalmanFilter5.jama.Matrix;
import com.zz.meridian.KalmanFilter5.jkalman.JKalman;

import java.util.Random;


/**
 * 测试类
 * JKalman TestBench
 */
public class KalmanTest {
    /**
     * Constructor
     */
    public KalmanTest() {
    }

    /**
     * Main method
     *
     * @param args
     */
    public static void main(String[] args) {
        try {
            /**
             * dynam_params 测量矢量维数
             * measure_params 状态矢量维数
             */
            JKalman kalman = new JKalman(4, 2);

            Random rand = new Random(System.currentTimeMillis() % 2011);
            double x = 0;
            double y = 0;
            // constant velocity 匀速 输入第一个点的 x坐标y坐标
            double dx = rand.nextDouble();
            double dy = rand.nextDouble();

            // init 初始化 生成4行1列坐标系
            Matrix s = new Matrix(4, 1); // 状态 state [x, y, dx, dy, dxy]
            Matrix c = new Matrix(4, 1); // corrected state [x, y, dx, dy, dxy]

            Matrix m = new Matrix(2, 1); // measurement [x]
            m.set(0, 0, x);
            m.set(1, 0, y);

            // transitions for x, y, dx, dy
            double[][] tr = {{1, 0, 1, 0},
                    {0, 1, 0, 1},
                    {0, 0, 1, 0},
                    {0, 0, 0, 1}};
            kalman.setTransition_matrix(new Matrix(tr));

            // 1s somewhere?
            kalman.setError_cov_post(kalman.getError_cov_post().identity());

            // init first assumption similar to first observation (cheat :)
            // kalman.setState_post(kalman.getState_post());

            // report what happend first :)
            System.out.println("第一位的first x:" + x + ", y:" + y + ", dx:" + dx + ", dy:" + dy);
            System.out.println("no; x; y; dx; dy; predictionX; predictionY; predictionDx; predictionDy; correctionX; correctionY; correctionDx; correctionDy;");

            // For debug only
            for (int i = 0; i < 200; ++i) {

                // check state before
                s = kalman.Predict();

                // function init :)
                // m.set(1, 0, rand.nextDouble());
                x = rand.nextGaussian();
                y = rand.nextGaussian();
                m.set(0, 0, m.get(0, 0) + dx + rand.nextGaussian());
                m.set(1, 0, m.get(1, 0) + dy + rand.nextGaussian());

                // a missing value (more then 1/4 times)
                if (rand.nextGaussian() < -0.8) {
                    System.out.println("" + i + ";;;;;"
                            + s.get(0, 0) + ";" + s.get(1, 0) + ";" + s.get(2, 0) + ";" + s.get(3, 0) + ";");
                } else { // measurement is ok :)
                    // look better
                    c = kalman.Correct(m);

                    System.out.println("" + i + ";" + m.get(0, 0) + ";" + m.get(1, 0) + ";" + x + ";" + y + ";"
                            + s.get(0, 0) + ";" + s.get(1, 0) + ";" + s.get(2, 0) + ";" + s.get(3, 0) + ";"
                            + c.get(0, 0) + ";" + c.get(1, 0) + ";" + c.get(2, 0) + ";" + c.get(3, 0) + ";");
                }

            }
        } catch (Exception ex) {
            System.out.println(ex.getMessage());
        }
    }
}

我自己写的1~3维测试类

package com.zz.meridian.KalmanFilter5;

import com.zz.meridian.KalmanFilter5.jama.Matrix;
import com.zz.meridian.KalmanFilter5.jkalman.JKalman;

import java.util.ArrayList;
import java.util.List;

/**
 * @author Tiger-l
 * 2022-08-09 19:49:27
 */
public class Main {
    public static void main(String[] args) throws Exception {
//        List<Float> stream=new ArrayList<>();
//        stream.add(2.1f);
//        stream.add(2.2f);
//        stream.add(2.3f);
//        stream.add(2.5f);
//        stream.add(2.8f);
//        stream.add(2.9f);
//        stream.add(3.1f);
//        stream.add(3.2f);
//        stream.add(3.4f);
//        stream.add(3.7f);
//        System.out.println("前"+stream);
//        List<Float> from1D = createFrom1D(stream);
//        System.out.println("后"+from1D);


//        List<float[]> stream=new ArrayList<>();
//        stream.add(new float[]{2.1f,1.1f});
//        stream.add(new float[]{2.2f,1.2f});
//        stream.add(new float[]{2.3f,1.3f});
//        stream.add(new float[]{2.5f,1.4f});
//        stream.add(new float[]{2.8f,1.7f});
//        stream.add(new float[]{2.9f,1.9f});
//        stream.add(new float[]{3.1f,2.1f});
//        stream.add(new float[]{3.2f,2.4f});
//        stream.add(new float[]{3.4f,2.5f});
//        stream.add(new float[]{3.7f,2.8f});
//        for (float[] floats : stream) {
//            System.out.print("["+ floats[0]+"-"+floats[1]+"]");
//        }
//        List<float[]> from1D = createFrom2D(stream);
//        System.out.println();
//        for (float[] floats : from1D) {
//            System.out.print("["+ floats[0]+"-"+floats[1]+"]");
//        }


//        List<float[]> stream=new ArrayList<>();
//        stream.add(new float[]{2.1f,1.1f,0.1f});
//        stream.add(new float[]{2.2f,1.2f,0.2f});
//        stream.add(new float[]{2.3f,1.3f,0.3f});
//        stream.add(new float[]{2.5f,1.4f,0.4f});
//        stream.add(new float[]{2.8f,1.7f,0.7f});
//        stream.add(new float[]{2.9f,1.9f,0.9f});
//        stream.add(new float[]{3.1f,2.1f,1.1f});
//        stream.add(new float[]{3.2f,2.4f,1.4f});
//        stream.add(new float[]{3.4f,2.5f,1.5f});
//        stream.add(new float[]{3.7f,2.8f,1.8f});
//        for (float[] floats : stream) {
//            System.out.print("["+ floats[0]+"-"+floats[1]+"-"+floats[2]+"]");
//        }
//        List<float[]> from1D = createFrom3D(stream);
//        System.out.println();
//        for (float[] floats : from1D) {
//            System.out.print("["+ floats[0]+"-"+floats[1]+"-"+floats[2]+"]");
//        }


        List<float[]> stream=new ArrayList<>();
        stream.add(new float[]{2.1f,1.1f,0.1f});
        stream.add(new float[]{2.2f,1.2f,0.2f});
        stream.add(new float[]{2.3f,1.3f,0.3f});
        stream.add(new float[]{2.5f,1.4f,0.4f});
        stream.add(new float[]{2.8f,1.7f,0.7f});
        stream.add(new float[]{2.9f,1.9f,0.9f});
        stream.add(new float[]{3.1f,2.1f,1.1f});
        stream.add(new float[]{3.2f,2.4f,1.4f});
        stream.add(new float[]{3.4f,2.5f,1.5f});
        stream.add(new float[]{3.7f,2.8f,1.8f});
        for (float[] floats : stream) {
            System.out.print("["+ floats[0]+"-"+floats[1]+"-"+floats[2]+"]");
        }
        List<float[]> from1D = createLowPassFilter(stream);
        System.out.println();
        for (float[] floats : from1D) {
            System.out.print("["+ floats[0]+"-"+floats[1]+"-"+floats[2]+"]");
        }
    }
    /**
     * Smoothens float value stream using kalman filter.
     * 利用卡尔曼滤波平滑浮点值流。
     * @param stream Float Stream.
     * @return Observable.
     */
    public static List<Float> createFrom1D(List<Float> stream) throws Exception {
        final JKalman kalman = new JKalman(2, 1);
        // measurement [x]
        final Matrix m = new Matrix(1, 1);

        // transitions for x, dx
        double[][] tr = {{1, 0},
                {0, 1}};
        kalman.setTransition_matrix(new Matrix(tr));

        // 1s somewhere?
        kalman.setError_cov_post(kalman.getError_cov_post().identity());

        List<Float> floats = new ArrayList<>();
        stream.stream().forEach(value -> {
            m.set(0, 0, value);
            // state [x, dx]
            Matrix s = kalman.Predict();
            // corrected state [x, dx]
            Matrix c = kalman.Correct(m);
            floats.add((float) c.get(0, 0));
        });
        return floats;
    }
    /**
     * Smoothens (float,float) value stream using kalman filter.
     * 平滑(浮，浮)值流使用卡尔曼滤波器
     * @param stream Float Stream.
     * @return Observable.
     */
    public static List<float[]> createFrom2D(List<float[]> stream) throws Exception {
        final JKalman kalman = new JKalman(4, 2);
        // measurement [x]
        final Matrix m = new Matrix(2, 1);
        // transitions for x, y, dx, dy
        double[][] tr = {{1, 0, 1, 0},
                {0, 1, 0, 1},
                {0, 0, 1, 0},
                {0, 0, 0, 1}};
        kalman.setTransition_matrix(new Matrix(tr));
        // 1s somewhere?
        kalman.setError_cov_post(kalman.getError_cov_post().identity());
        final float[] buffer = new float[2];
        List<float[]> floats = new ArrayList<>();
        stream.stream().forEach(values -> {
            m.set(0, 0, values[0]);
            m.set(1, 0, values[1]);
            // state [x, dx]
            Matrix s = kalman.Predict();
            // corrected state [x, dx]
            Matrix c = kalman.Correct(m);
            buffer[0] = (float) c.get(0, 0);
            buffer[1] = (float) c.get(1, 0);
            floats.add(buffer);
        });
        return floats;
    }

    /**
     * Smoothens (float,float,float) value stream using kalman filter.
     * 使用卡尔曼滤波平滑(浮，浮，浮)值流
     * @param stream Float Stream.
     * @return Observable.
     */
    public static List<float[]> createFrom3D(List<float[]> stream) throws Exception {
        final JKalman kalman = new JKalman(6, 3);
        // measurement [x, y, z]
        Matrix m = new Matrix(3, 1);
        // transitions for x, y, z, dx, dy, dz (velocity transitions)
        double[][] tr = {{1, 0, 0, 1, 0, 0},
                {0, 1, 0, 0, 1, 0},
                {0, 0, 1, 0, 0, 1},
                {0, 0, 0, 1, 0, 0},
                {0, 0, 0, 0, 1, 0},
                {0, 0, 0, 0, 0, 1}};
        kalman.setTransition_matrix(new Matrix(tr));
        // 1s somewhere?
        kalman.setError_cov_post(kalman.getError_cov_post().identity());
        final float[] buffer = new float[3];
        List<float[]> floats = new ArrayList<>();
        stream.stream().forEach(values -> {
            m.set(0, 0, values[0]);
            m.set(1, 0, values[1]);
            m.set(2, 0, values[2]);
            // state [x, y, z, dx, dy, dz]
            Matrix s = kalman.Predict();
            // corrected state [x, y,z, dx, dy, dz, dxyz]
            Matrix c = kalman.Correct(m);
            buffer[0] = (float) c.get(0, 0);
            buffer[1] = (float) c.get(1, 0);
            buffer[2] = (float) c.get(2, 0);
            floats.add(buffer);
        });
        return floats;
    }
    /**
     * Applies low pass filter for (float,float,float) value stream.
     * 对(float,float,float)值流应用低通滤波器-卡尔曼滤波器的默认实现是一个迭代过程，使用上一次的结果预测当前的值，同时使用观测值修正当前值，得到最优结果
     * @param stream Float Stream.
     * @return Observable.
     */
    public static List<float[]> createLowPassFilter(List<float[]> stream) {
        return createLowPassFilter(stream, 0.8f);
    }
    public static List<float[]> createLowPassFilter(List<float[]> stream, final float alpha) {
        final float[] output = new float[3];
        final float[] gravity = new float[3];
        List<float[]> floats = new ArrayList<>();
        stream.stream().forEach(values -> {
            // skip invalid values
            if (values == null || values.length != 3)
                return;
            // apply low pass filter
            applyLowPassFilter(values, output, gravity, alpha);
            // pass values
            floats.add(output);
        });
        return floats;
    }
    /**
     * In this example, alpha is calculated as t / (t + dT),
     * where t is the low-pass filter's time-constant and
     * dT is the event delivery rate.
     * 在这个例子中，alpha被计算为t/ (t + dT)，其中t是Low-pass过滤器的时间常数，dT是事件交付率。
     */
    static void applyLowPassFilter(float[] input, float[] output, float[] gravity, float alpha) {
        // Isolate the force of gravity with the low-pass filter.
        gravity[0] = alpha * gravity[0] + (1 - alpha) * input[0];
        gravity[1] = alpha * gravity[1] + (1 - alpha) * input[1];
        gravity[2] = alpha * gravity[2] + (1 - alpha) * input[2];
        // Remove the gravity contribution with the high-pass filter.
        output[0] = input[0] - gravity[0];
        output[1] = input[1] - gravity[1];
        output[2] = input[2] - gravity[2];
    }
}

tiger_angel

关注

1
点赞
踩
14

收藏

觉得还不错? 一键收藏
0
评论
Kalman Filter卡尔曼滤波 java实现

卡尔曼滤波就是-测量值与预测值之间取最优结果-得到最优结果卡尔曼滤波就是-上一次最优结果预测当前的值，同时使用观测者修正当前值，得到最优结果列子：再汽车行驶途中，如何确认定位自己所在位置？提供的数据：1.已知加速度信息，2里程表信息，3GPS信息。三种数据都存在误差说明：卡尔曼滤波结合已知信息，估计最优位置，本质是优化估计算法，例如估计人在下一帧的位置。说白了综合已知数据取最优值，阿波罗登月这哥们也用上了计算轨迹。状态向量:(位置和速递)位置速度-1 上一秒的位置。.........
复制链接

扫一扫