• Kalman Filter卡尔曼滤波 java实现

    我的理解

    重点-理解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  加速度乘以时间

    推荐使用第三种,前两种不保证正确

    第一种:

    1. package com.zz.meridian.KalmanFilter;
    2.  
    3.  
    4. import java.util.ArrayList;
    5.  
    6. /**
    7.  * 重点：测量值与预测值之间取最优结果-让算法去算取折中的最优值
    8.  *
    9.  */
    10. public class KalmanFilter {
    11.  
    12.     /**Kalman Filter*/
    13.     private Integer predict; //观察数据值
    14.     private Integer current; //观察数据值的下一条数据
    15.     private Integer estimate;//每一次计算出的-最终估计值
    16.     private double pdelt; //系统测量误差-为计算高斯噪声方差
    17.     private double mdelt; //系统测量误差-为计算高斯噪声方差
    18.     private double Gauss; //高斯噪声方差
    19.     private double kalmanGain;//估计方差
    20.     //信任程度 因为实际中不同传感器精度不同昂贵的高精度传感器就可以更信任一些Ｒ可以小一些。  或者我建立的模型很优秀误差极小就可以更信任模型Ｑ可以小一些
    21.     /*
    22.     QR：
    23.         Q模型误差与R测量误差的大小，是模型预测值与测量值的加权
    24.             R固定，Q越大，代表越信任侧量值，
    25.             Q无穷代表只用测量值；反之，
    26.             Q越小代表越信任模型预测值，Q为零则是只用模型预测
    27.         Q是系统过程噪声的协方差矩阵，而R则是观测噪声的协方差矩阵。后者和你选择的传感器息息相关,R是看传感器精度，Q是过程误差看环境的影响大不大，我一般Q取0.01
    28.         R为大于0常数都可以 比如1. P初始值设为足够大的对角矩阵。Q大小影响收敛速度。可以试验几个数值。
    29.         Q和R分别代表对预测值和测量值的置信度（反比），通过影响卡尔曼增益K的值，影响预测值和测量值的权重。越大的R代表越不相信测量值。
    30.         q越小，越依赖系统模型，r越小，越依赖观测值
    31.      */
    32.     private final static double Q = 0.00001; //（自定义-调参用）
    33.     private final static double R = 0.1; //（自定义-调参用
    34.  
    35.     public void initial(){
    36. //        pdelt = 4;    //系统测量误差
    37. //        mdelt = 3;
    38.         pdelt = 4;   //系统测量误差
    39.         mdelt = 3;  //估计方差
    40.     }
    41.     public Integer KalmanFilter(Integer oldValue,Integer value){
    42.         //(1)第一个估计值
    43.         predict = oldValue;
    44.         //第二个估计值
    45.         current = value;
    46.         //(2)高斯噪声方差
    47.         Gauss = Math.sqrt(pdelt * pdelt + mdelt * mdelt) + Q;
    48.         //(3)估计方差
    49.         kalmanGain = Math.sqrt((Gauss * Gauss)/(Gauss * Gauss + pdelt * pdelt)) + R;
    50.         //(4)最终估计值
    51.         estimate = (int) (kalmanGain * (current - predict) + predict);
    52.         //(5)新的估计方差，下一次不确定性的度量
    53.         mdelt = Math.sqrt((1-kalmanGain) * Gauss * Gauss);
    54.  
    55.         return estimate;
    56.     }
    57.  
    58.     public static void main(String[] args) {
    59.         KalmanFilter kalmanfilter =new KalmanFilter();
    60.         kalmanfilter.initial();
    61.         ArrayList list = new ArrayList();
    62.         list.add(-75);
    63.         list.add(-76);
    64.         list.add(-81);
    65.         list.add(-75);
    66.         list.add(-77);
    67.         list.add(-76);
    68.         list.add(-86);
    69.  
    70.         int oldvalue = list.get(0);
    71.         ArrayList alist = new ArrayList();
    72.         for(int i = 0; i < list.size(); i++){
    73.             int value = list.get(i);
    74.             oldvalue = kalmanfilter.KalmanFilter(oldvalue,value);
    75.             alist.add(oldvalue);
    76.         }
    77.  
    78.         System.out.println(list);
    79.         System.out.println(alist);
    80.  
    81.     }
    82.  
    83. }

    第二种:

    1. package com.zz.meridian.KalmanFilter3;
    2.  
    3. /**
    4.  * 卡尔曼滤波
    5.  */
    6.  
    7. import java.util.ArrayList;
    8.  
    9. /*
    10. 关于卡尔曼滤波器的原理，网上有很多，这里就不做过多介绍，此demo为一阶卡尔曼滤波器的实现。
    11. 主要为五个公式 （后面为一阶滤波器的系数）
    12. X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k), A=1,BU(k) = 0
    13. P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1
    14. Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1
    15. X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1
    16. P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1
    17.  */
    18. public class KalmanFilter {
    19.  
    20.     private final double Q = 0.000001;
    21.     private final double R = 0.001;
    22.     private ArrayList dataArrayList;
    23.     private int length;
    24.  
    25.     private double z[]; // data
    26.     private double xhat[];
    27.     private double xhatminus[];
    28.     private double P[];
    29.     private double Pminus[];
    30.     private double K[];
    31.  
    32.     public KalmanFilter(ArrayList arrayList) {
    33.         this.dataArrayList = arrayList;
    34.         this.length = arrayList.size();
    35.         z = new double[length];
    36.         xhat = new double[length];
    37.         xhatminus = new double[length];
    38.         P = new double[length];
    39.         Pminus = new double[length];
    40.         K = new double[length];
    41.         xhat[0] = 0;
    42.         P[0] = 1.0;
    43.  
    44.         for (int i = 0; i < length; i++) {
    45.             z[i] = (double) dataArrayList.get(i);
    46.         }
    47.     }
    48.  
    49.     public ArrayList calc() {
    50.         if (dataArrayList.size() < 2) {
    51.             return dataArrayList;
    52.         }
    53.         for (int k = 1; k < length; k++) {
    54.             // X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k),A=1,BU(k) = 0
    55.             xhatminus[k] = xhat[k - 1];
    56.  
    57.             // P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1
    58.             Pminus[k] = P[k - 1] + Q;
    59.  
    60.             // Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1
    61.             K[k] = Pminus[k] / (Pminus[k] + R);
    62.  
    63.             // X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1
    64.             xhat[k] = xhatminus[k] + K[k] * (z[k] - xhatminus[k]);
    65.  
    66.             //P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1
    67.             P[k] = (1 - K[k]) * Pminus[k];
    68.         }
    69.  
    70.         for (int i = 0; i < length; i++) {
    71.             dataArrayList.set(i, xhat[i]);
    72.         }
    73.         dataArrayList.remove(0);
    74.         return dataArrayList;
    75.     }
    76.  
    77.     public static void main(String[] args) {
    78.         ArrayList list = new ArrayList<>();
    79.         list.add(-75.);
    80.         list.add(-76.);
    81.         list.add(-81.);
    82.         list.add(-75.);
    83.         list.add(-77.);
    84.         list.add(-76.);
    85.         list.add(-86.);
    86.         System.err.println("滤波前的:"+list);
    87.         KalmanFilter kalmanFilter = new KalmanFilter(list);
    88.         kalmanFilter.calc();
    89.         System.err.println("滤波后的:"+kalmanFilter.dataArrayList);
    90.     }
    91. }

    第三种JKalman代码:

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

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

    外网不行,下载这个

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

     官方JKalman测试类

    1. /*******************************************************************************
    2.  *
    3.  *  JKalman - KALMAN FILTER (Java) TestBench
    4.  *
    5.  *  Copyright (C) 2007 Petr Chmelar
    6.  *
    7.  *  By downloading, copying, installing or using the software you agree to
    8.  *  the license in licenseIntel.txt or in licenseGNU.txt
    9.  *
    10.  **************************************************************************** */
    11.  
    12. package com.zz.meridian.KalmanFilter5;
    13.  
    14. import com.zz.meridian.KalmanFilter5.jama.Matrix;
    15. import com.zz.meridian.KalmanFilter5.jkalman.JKalman;
    16.  
    17. import java.util.Random;
    18.  
    19.  
    20. /**
    21.  * 测试类
    22.  * JKalman TestBench
    23.  */
    24. public class KalmanTest {
    25.     /**
    26.      * Constructor
    27.      */
    28.     public KalmanTest() {
    29.     }
    30.  
    31.     /**
    32.      * Main method
    33.      *
    34.      * @param args
    35.      */
    36.     public static void main(String[] args) {
    37.         try {
    38.             /**
    39.              * dynam_params 测量矢量维数
    40.              * measure_params 状态矢量维数
    41.              */
    42.             JKalman kalman = new JKalman(4, 2);
    43.  
    44.             Random rand = new Random(System.currentTimeMillis() % 2011);
    45.             double x = 0;
    46.             double y = 0;
    47.             // constant velocity 匀速 输入第一个点的 x坐标y坐标
    48.             double dx = rand.nextDouble();
    49.             double dy = rand.nextDouble();
    50.  
    51.             // init 初始化 生成4行1列坐标系
    52.             Matrix s = new Matrix(4, 1); // 状态 state [x, y, dx, dy, dxy]
    53.             Matrix c = new Matrix(4, 1); // corrected state [x, y, dx, dy, dxy]
    54.  
    55.             Matrix m = new Matrix(2, 1); // measurement [x]
    56.             m.set(0, 0, x);
    57.             m.set(1, 0, y);
    58.  
    59.             // transitions for x, y, dx, dy
    60.             double[][] tr = {{1, 0, 1, 0},
    61.                     {0, 1, 0, 1},
    62.                     {0, 0, 1, 0},
    63.                     {0, 0, 0, 1}};
    64.             kalman.setTransition_matrix(new Matrix(tr));
    65.  
    66.             // 1s somewhere?
    67.             kalman.setError_cov_post(kalman.getError_cov_post().identity());
    68.  
    69.             // init first assumption similar to first observation (cheat :)
    70.             // kalman.setState_post(kalman.getState_post());
    71.  
    72.             // report what happend first :)
    73.             System.out.println("第一位的first x:" + x + ", y:" + y + ", dx:" + dx + ", dy:" + dy);
    74.             System.out.println("no; x; y; dx; dy; predictionX; predictionY; predictionDx; predictionDy; correctionX; correctionY; correctionDx; correctionDy;");
    75.  
    76.             // For debug only
    77.             for (int i = 0; i < 200; ++i) {
    78.  
    79.                 // check state before
    80.                 s = kalman.Predict();
    81.  
    82.                 // function init :)
    83.                 // m.set(1, 0, rand.nextDouble());
    84.                 x = rand.nextGaussian();
    85.                 y = rand.nextGaussian();
    86.                 m.set(0, 0, m.get(0, 0) + dx + rand.nextGaussian());
    87.                 m.set(1, 0, m.get(1, 0) + dy + rand.nextGaussian());
    88.  
    89.                 // a missing value (more then 1/4 times)
    90.                 if (rand.nextGaussian() < -0.8) {
    91.                     System.out.println("" + i + ";;;;;"
    92.                             + s.get(0, 0) + ";" + s.get(1, 0) + ";" + s.get(2, 0) + ";" + s.get(3, 0) + ";");
    93.                 } else { // measurement is ok :)
    94.                     // look better
    95.                     c = kalman.Correct(m);
    96.  
    97.                     System.out.println("" + i + ";" + m.get(0, 0) + ";" + m.get(1, 0) + ";" + x + ";" + y + ";"
    98.                             + s.get(0, 0) + ";" + s.get(1, 0) + ";" + s.get(2, 0) + ";" + s.get(3, 0) + ";"
    99.                             + c.get(0, 0) + ";" + c.get(1, 0) + ";" + c.get(2, 0) + ";" + c.get(3, 0) + ";");
    100.                 }
    101.  
    102.             }
    103.         } catch (Exception ex) {
    104.             System.out.println(ex.getMessage());
    105.         }
    106.     }
    107. }

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

    1. package com.zz.meridian.KalmanFilter5;
    2.  
    3. import com.zz.meridian.KalmanFilter5.jama.Matrix;
    4. import com.zz.meridian.KalmanFilter5.jkalman.JKalman;
    5.  
    6. import java.util.ArrayList;
    7. import java.util.List;
    8.  
    9. /**
    10.  * @author Tiger-l
    11.  * 2022-08-09 19:49:27
    12.  */
    13. public class Main {
    14.     public static void main(String[] args) throws Exception {
    15. //        List stream=new ArrayList<>();
    16. //        stream.add(2.1f);
    17. //        stream.add(2.2f);
    18. //        stream.add(2.3f);
    19. //        stream.add(2.5f);
    20. //        stream.add(2.8f);
    21. //        stream.add(2.9f);
    22. //        stream.add(3.1f);
    23. //        stream.add(3.2f);
    24. //        stream.add(3.4f);
    25. //        stream.add(3.7f);
    26. //        System.out.println("前"+stream);
    27. //        List from1D = createFrom1D(stream);
    28. //        System.out.println("后"+from1D);
    29.  
    30.  
    31. //        List stream=new ArrayList<>();
    32. //        stream.add(new float[]{2.1f,1.1f});
    33. //        stream.add(new float[]{2.2f,1.2f});
    34. //        stream.add(new float[]{2.3f,1.3f});
    35. //        stream.add(new float[]{2.5f,1.4f});
    36. //        stream.add(new float[]{2.8f,1.7f});
    37. //        stream.add(new float[]{2.9f,1.9f});
    38. //        stream.add(new float[]{3.1f,2.1f});
    39. //        stream.add(new float[]{3.2f,2.4f});
    40. //        stream.add(new float[]{3.4f,2.5f});
    41. //        stream.add(new float[]{3.7f,2.8f});
    42. //        for (float[] floats : stream) {
    43. //            System.out.print("["+ floats[0]+"-"+floats[1]+"]");
    44. //        }
    45. //        List from1D = createFrom2D(stream);
    46. //        System.out.println();
    47. //        for (float[] floats : from1D) {
    48. //            System.out.print("["+ floats[0]+"-"+floats[1]+"]");
    49. //        }
    50.  
    51.  
    52. //        List stream=new ArrayList<>();
    53. //        stream.add(new float[]{2.1f,1.1f,0.1f});
    54. //        stream.add(new float[]{2.2f,1.2f,0.2f});
    55. //        stream.add(new float[]{2.3f,1.3f,0.3f});
    56. //        stream.add(new float[]{2.5f,1.4f,0.4f});
    57. //        stream.add(new float[]{2.8f,1.7f,0.7f});
    58. //        stream.add(new float[]{2.9f,1.9f,0.9f});
    59. //        stream.add(new float[]{3.1f,2.1f,1.1f});
    60. //        stream.add(new float[]{3.2f,2.4f,1.4f});
    61. //        stream.add(new float[]{3.4f,2.5f,1.5f});
    62. //        stream.add(new float[]{3.7f,2.8f,1.8f});
    63. //        for (float[] floats : stream) {
    64. //            System.out.print("["+ floats[0]+"-"+floats[1]+"-"+floats[2]+"]");
    65. //        }
    66. //        List from1D = createFrom3D(stream);
    67. //        System.out.println();
    68. //        for (float[] floats : from1D) {
    69. //            System.out.print("["+ floats[0]+"-"+floats[1]+"-"+floats[2]+"]");
    70. //        }
    71.  
    72.  
    73.         List<float[]> stream=new ArrayList<>();
    74.         stream.add(new float[]{2.1f,1.1f,0.1f});
    75.         stream.add(new float[]{2.2f,1.2f,0.2f});
    76.         stream.add(new float[]{2.3f,1.3f,0.3f});
    77.         stream.add(new float[]{2.5f,1.4f,0.4f});
    78.         stream.add(new float[]{2.8f,1.7f,0.7f});
    79.         stream.add(new float[]{2.9f,1.9f,0.9f});
    80.         stream.add(new float[]{3.1f,2.1f,1.1f});
    81.         stream.add(new float[]{3.2f,2.4f,1.4f});
    82.         stream.add(new float[]{3.4f,2.5f,1.5f});
    83.         stream.add(new float[]{3.7f,2.8f,1.8f});
    84.         for (float[] floats : stream) {
    85.             System.out.print("["+ floats[0]+"-"+floats[1]+"-"+floats[2]+"]");
    86.         }
    87.         List<float[]> from1D = createLowPassFilter(stream);
    88.         System.out.println();
    89.         for (float[] floats : from1D) {
    90.             System.out.print("["+ floats[0]+"-"+floats[1]+"-"+floats[2]+"]");
    91.         }
    92.     }
    93.     /**
    94.      * Smoothens float value stream using kalman filter.
    95.      * 利用卡尔曼滤波平滑浮点值流。
    96.      * @param stream Float Stream.
    97.      * @return Observable.
    98.      */
    99.     public static List createFrom1D(List stream) throws Exception {
    100.         final JKalman kalman = new JKalman(2, 1);
    101.         // measurement [x]
    102.         final Matrix m = new Matrix(1, 1);
    103.  
    104.         // transitions for x, dx
    105.         double[][] tr = {{1, 0},
    106.                 {0, 1}};
    107.         kalman.setTransition_matrix(new Matrix(tr));
    108.  
    109.         // 1s somewhere?
    110.         kalman.setError_cov_post(kalman.getError_cov_post().identity());
    111.  
    112.         List floats = new ArrayList<>();
    113.         stream.stream().forEach(value -> {
    114.             m.set(0, 0, value);
    115.             // state [x, dx]
    116.             Matrix s = kalman.Predict();
    117.             // corrected state [x, dx]
    118.             Matrix c = kalman.Correct(m);
    119.             floats.add((float) c.get(0, 0));
    120.         });
    121.         return floats;
    122.     }
    123.     /**
    124.      * Smoothens (float,float) value stream using kalman filter.
    125.      * 平滑(浮，浮)值流使用卡尔曼滤波器
    126.      * @param stream Float Stream.
    127.      * @return Observable.
    128.      */
    129.     public static List<float[]> createFrom2D(List<float[]> stream) throws Exception {
    130.         final JKalman kalman = new JKalman(4, 2);
    131.         // measurement [x]
    132.         final Matrix m = new Matrix(2, 1);
    133.         // transitions for x, y, dx, dy
    134.         double[][] tr = {{1, 0, 1, 0},
    135.                 {0, 1, 0, 1},
    136.                 {0, 0, 1, 0},
    137.                 {0, 0, 0, 1}};
    138.         kalman.setTransition_matrix(new Matrix(tr));
    139.         // 1s somewhere?
    140.         kalman.setError_cov_post(kalman.getError_cov_post().identity());
    141.         final float[] buffer = new float[2];
    142.         List<float[]> floats = new ArrayList<>();
    143.         stream.stream().forEach(values -> {
    144.             m.set(0, 0, values[0]);
    145.             m.set(1, 0, values[1]);
    146.             // state [x, dx]
    147.             Matrix s = kalman.Predict();
    148.             // corrected state [x, dx]
    149.             Matrix c = kalman.Correct(m);
    150.             buffer[0] = (float) c.get(0, 0);
    151.             buffer[1] = (float) c.get(1, 0);
    152.             floats.add(buffer);
    153.         });
    154.         return floats;
    155.     }
    156.  
    157.     /**
    158.      * Smoothens (float,float,float) value stream using kalman filter.
    159.      * 使用卡尔曼滤波平滑(浮，浮，浮)值流
    160.      * @param stream Float Stream.
    161.      * @return Observable.
    162.      */
    163.     public static List<float[]> createFrom3D(List<float[]> stream) throws Exception {
    164.         final JKalman kalman = new JKalman(6, 3);
    165.         // measurement [x, y, z]
    166.         Matrix m = new Matrix(3, 1);
    167.         // transitions for x, y, z, dx, dy, dz (velocity transitions)
    168.         double[][] tr = {{1, 0, 0, 1, 0, 0},
    169.                 {0, 1, 0, 0, 1, 0},
    170.                 {0, 0, 1, 0, 0, 1},
    171.                 {0, 0, 0, 1, 0, 0},
    172.                 {0, 0, 0, 0, 1, 0},
    173.                 {0, 0, 0, 0, 0, 1}};
    174.         kalman.setTransition_matrix(new Matrix(tr));
    175.         // 1s somewhere?
    176.         kalman.setError_cov_post(kalman.getError_cov_post().identity());
    177.         final float[] buffer = new float[3];
    178.         List<float[]> floats = new ArrayList<>();
    179.         stream.stream().forEach(values -> {
    180.             m.set(0, 0, values[0]);
    181.             m.set(1, 0, values[1]);
    182.             m.set(2, 0, values[2]);
    183.             // state [x, y, z, dx, dy, dz]
    184.             Matrix s = kalman.Predict();
    185.             // corrected state [x, y,z, dx, dy, dz, dxyz]
    186.             Matrix c = kalman.Correct(m);
    187.             buffer[0] = (float) c.get(0, 0);
    188.             buffer[1] = (float) c.get(1, 0);
    189.             buffer[2] = (float) c.get(2, 0);
    190.             floats.add(buffer);
    191.         });
    192.         return floats;
    193.     }
    194.     /**
    195.      * Applies low pass filter for (float,float,float) value stream.
    196.      * 对(float,float,float)值流应用低通滤波器-卡尔曼滤波器的默认实现是一个迭代过程，使用上一次的结果预测当前的值，同时使用观测值修正当前值，得到最优结果
    197.      * @param stream Float Stream.
    198.      * @return Observable.
    199.      */
    200.     public static List<float[]> createLowPassFilter(List<float[]> stream) {
    201.         return createLowPassFilter(stream, 0.8f);
    202.     }
    203.     public static List<float[]> createLowPassFilter(List<float[]> stream, final float alpha) {
    204.         final float[] output = new float[3];
    205.         final float[] gravity = new float[3];
    206.         List<float[]> floats = new ArrayList<>();
    207.         stream.stream().forEach(values -> {
    208.             // skip invalid values
    209.             if (values == null || values.length != 3)
    210.                 return;
    211.             // apply low pass filter
    212.             applyLowPassFilter(values, output, gravity, alpha);
    213.             // pass values
    214.             floats.add(output);
    215.         });
    216.         return floats;
    217.     }
    218.     /**
    219.      * In this example, alpha is calculated as t / (t + dT),
    220.      * where t is the low-pass filter's time-constant and
    221.      * dT is the event delivery rate.
    222.      * 在这个例子中，alpha被计算为t/ (t + dT)，其中t是Low-pass过滤器的时间常数，dT是事件交付率。
    223.      */
    224.     static void applyLowPassFilter(float[] input, float[] output, float[] gravity, float alpha) {
    225.         // Isolate the force of gravity with the low-pass filter.
    226.         gravity[0] = alpha * gravity[0] + (1 - alpha) * input[0];
    227.         gravity[1] = alpha * gravity[1] + (1 - alpha) * input[1];
    228.         gravity[2] = alpha * gravity[2] + (1 - alpha) * input[2];
    229.         // Remove the gravity contribution with the high-pass filter.
    230.         output[0] = input[0] - gravity[0];
    231.         output[1] = input[1] - gravity[1];
    232.         output[2] = input[2] - gravity[2];
    233.     }
    234. }
  • 相关阅读:
    MST2101Q2 摩托车三相磁电机调压器控制芯片
    SpringBoot自定义参数解析器HandlerMethodArgumentResolver（解析ip）
    系统管理员道德规范
    Web Worker
    【Effective Modern Cpp】条款9:优先考虑别名声明而非typedef
    【单例模式 Objective-C语言】
    气相催化还原二氧化硫影响因素实验研究
    计算机毕业设计springboot+vue基本微信小程序的考试系统
    lightGBM有效因子筛选与qlib自定义handler
    数据结构与算法--其他算法
  • 原文地址：https://blog.csdn.net/qq_38567039/article/details/126243605