zoukankan      html  css  js  c++  java
  • kalman处理realsense数据

    代码来自:https://www.cnblogs.com/zjuhjm/archive/2012/12/29/2838472.html

    import numpy as np
    import matplotlib.pyplot as plt
    Q = 0.00001
    R = 0.1
    P_k_k1 = 1
    Kg = 0
    P_k1_k1 = 1
    x_k_k1 = 0
    ADC_OLD_Value = 0

    kalman_adc_old = 0
    def kalman(ADC_Value):
      global kalman_adc_old
      global P_k1_k1
      Z_k = ADC_Value

      if (abs(kalman_adc_old-ADC_Value)>=30):
        x_k1_k1= ADC_Value*0.382 + kalman_adc_old*0.618
      else:
        x_k1_k1 = kalman_adc_old;

      x_k_k1 = x_k1_k1
      P_k_k1 = P_k1_k1 + Q

      Kg = P_k_k1/(P_k_k1 + R)

      kalman_adc = x_k_k1 + Kg * (Z_k - kalman_adc_old)
      P_k1_k1 = (1 - Kg)*P_k_k1
      P_k_k1 = P_k1_k1

      ADC_OLD_Value = ADC_Value
      kalman_adc_old = kalman_adc
      return kalman_adc


    a= [100]*200
    array = np.array(a)

    s = np.random.normal(0, 25, 200)

    #test_array = array + s

    test_array = [0.618,0.672,0.905,0.985,1.028,1.255,1.320,1.396,1.724,1.753,1.769,2.091,2.113,2.367,2.414,2.817,2.840,2.897,3.073,3.254,3.289,4.717,4.407,5.113,5.200,5.091,4.967,5.580,5.888,6.338,6.473,6.542,5.795,5.907,5.850,4.987,5.028,5.028,4.987,5.291,5.084,5.156,5.481,5.200,4.507,4.575,4.541,3.457,3.450,3.477,2.884,2.596,2.983,3.012,2.753,2.468,2.453,1.358,1.281,1.249,0.908]

    plt.plot(test_array)
    adc=[]
    for i in range(len(test_array)):
      adc.append(kalman(test_array[i]))


    plt.plot(adc)
    plt.title('kalman')
    #plt.plot(array)

  • 相关阅读:
    github系列:gh-pages
    js 传参乱码问题解决
    nvm安装
    如何创建.babelrc文件?
    Hello World
    python——字符串截取
    python读入写入中文名图片
    VGG16等keras预训练权重文件的下载及本地存放
    在Linux服务器非root权限下搭建TensorFlow框架(Anaconda)
    查看linux服务器上Tensorflow的版本和位置
  • 原文地址:https://www.cnblogs.com/herd/p/9832621.html
Copyright © 2011-2022 走看看