Kalman Filter卡尔曼滤波 java实现
2024-05-07 07:18:15
我的理解
重点-理解1：卡尔曼滤波就是-测量值与预测值之间取最优结果-得到最优结果
重点-理解2：卡尔曼滤波就是-上一次最优结果预测当前的值，同时使用观测者修正当前值，得到最优结果
列子：再汽车行驶途中，如何确认定位自己所在位置？
提供的数据：1.已知加速度信息，2里程表信息，3GPS信息。三种数据都存在误差
说明：卡尔曼滤波结合已知信息，估计最优位置，本质是优化估计算法，例如估计人在下一帧的位置。说白了综合已知数据取最优值，阿波罗登月这哥们也用上了计算轨迹。
状态向量:(位置和速递)
位置
速度
-1 上一秒的位置
-1 上一秒的速度
时间
(这儿小车的速度是一样的)小车加速度
小车上一秒和下一秒的加速度
加速度乘以时间
推荐使用第三种,前两种不保证正确
第一种:
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.01R为大于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[]; // dataprivate 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) = 0xhatminus[k] = xhat[k - 1];// P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1Pminus[k] = P[k - 1] + Q;// Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1K[k] = Pminus[k] / (Pminus[k] + R);// X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1xhat[k] = xhatminus[k] + K[k] * (z[k] - xhatminus[k]);//P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1P[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, dydouble[][] 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 onlyfor (int i = 0; i < 200; ++i) {// check state befores = 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 betterc = 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, dxdouble[][] 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, dydouble[][] 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 valuesif (values == null || values.length != 3)return;// apply low pass filterapplyLowPassFilter(values, output, gravity, alpha);// pass valuesfloats.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];}
}
Kalman Filter卡尔曼滤波 java实现相关推荐
- Kalman Filter --卡尔曼滤波
Kalman Filter --卡尔曼滤波 简介 此处借鉴百度百科 卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法.由 ...
- 一文图解卡尔曼滤波(Kalman Filter)
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 译者注:这恐怕是全网有关卡尔曼滤波最简单易懂的解释,如果你认真的读 ...
- 图解卡尔曼滤波(Kalman Filter)
背景 关于滤波 首先援引来自知乎大神的解释. "一位专业课的教授给我们上课的时候,曾谈到:filtering is weighting(滤波即加权).滤波的作用就是给不同的信号分量不同的权重 ...
- matlab温度数据怎么滤波_卡尔曼滤波算法思想理解 Kalman filter 第一篇
卡尔曼滤波算法思想理解 Kalman filter 第一篇 最近在初步的理解目标跟踪的领域, 其中一个非常经典的算法卡尔曼滤波Kalman filter是需要有很好的理解才行, 由于已经脱离了学校,懂 ...
- 【UWB】Kalman filter, KF卡尔曼滤波, EKF 扩展卡尔曼滤波
文章目录 卡尔曼滤波器 扩展卡尔曼滤波器 协方差 Ref: 卡尔曼滤波器 首先从工程上看卡尔曼滤波算法. 引入一个离散控制过程的系统,该系统可用一个线性随机微分方程(linear stochastic ...
- 通俗的解释卡尔曼滤波（Kalman Filter）以及其Python的实现
卡尔曼滤波 风力发电机中的风速估计,转速估计甚至扭矩估计都设计到卡尔曼滤波,如果只是单一传感变量的平滑处理也能用到卡尔曼滤波. 振动信号中的滤波大多采用低通去除高频噪音,而卡尔曼滤波则是通过不确定度把 ...
- 卡尔曼滤波算法-Kalman Filter Algorithm
1.简介 1.1 滤波是什么 所谓了滤波,就是从混合在一起的诸多信号中提取出所需要的信号. 1.2 信号的分类: (1)确定性信号:可以表示为确定的时间函数,可确定其在任何时刻的量值.(具有确定的频谱 ...
- 卡尔曼滤波（Kalman Filter）原理理解和测试
Kalman Filter学原理学习 1. Kalman Filter 历史 Kalman滤波器的历史,最早要追溯到17世纪,Roger Cotes开始研究最小均方问题.但由于缺少实际案例的支撑(那个 ...
- 多目标跟踪（MOT）中的卡尔曼滤波（Kalman filter）和匈牙利(Hungarian)算法详解
多目标跟踪(MOT)中的卡尔曼滤波(Kalman filter)和匈牙利(Hungarian)算法详解 1. 概览 在开始具体讨论卡尔曼滤波和匈牙利算法之前,首先我们来看一下基于检测的目标跟踪算法的大 ...
最新文章
- 局部刷新时间 jsp_局部区块多个报表 TAB 页切换及局部区块的参数查询
- 2.Java异常学习
- 类与类之间关系的表示方式
- linux db2乱码,DB2乱码(开始和结束，字符串中间没有好的办法)
- 4.1 多层感知机从0开始 4.2 多层感知机简洁实现(API调用)
- Bootstrap的响应式,当文字超过div长度,换行问题的处理!
- 利用AutoHotkey实现Vim和Excel的数据传递
- 合并k个有序链表 python_leetcode第23题-合并K个有序链表
- c语言程序设计实训教材,C语言程序设计实训指导书
- ArrayQueue详解(待解决)
- java lock unlock_【Java并发007】原理层面：ReentrantLock中lock()、unlock()全解析
- 执行sql报错：Incorrect table definition； there can be only one TIMESTAMP column with CURRENT_TIMESTAMP in
- 开发Shell脚本解决DOS安全Linux服务器生产案例
- 数据-第16课-栈的应用实战二
- 盘古搜索：上市是既定策略 寻求股权多元化
- UVA - 10041 Vito's Family (中位数)
- P1359 租用游艇（dijkstra不优化）
- YOLO3-WIN10-GPU版配置详细教程
- 量子计算机基本信息单位,单粒子的量子态可作为存储最基本单位
- 太可怕啦！AI 一秒还原马赛克,有码变高清