donkarlo

donkarlo@donkarlo-uni-laptop:~/Dropbox/projs/research/kalman-filter$ grep -r "operator<<" ./
Binary file ./cmake-build-debug/kalman-test matches
Binary file ./cmake-build-debug/CMakeFiles/kalman-test.dir/kalman.cpp.o matches
Binary file ./cmake-build-debug/CMakeFiles/kalman-test.dir/kalman-test.cpp.o matches

donkarlo

KalmanFilter::KalmanFilter(
    double dt,
    Eigen::MatrixXd processMatrix,
    Eigen::MatrixXd obsMtx,
    Eigen::MatrixXd processNoiseCov,
    Eigen::MatrixXd obsNoiseCov,
    Eigen::MatrixXd estimatErrCov)
    : processMtx(processMatrix), obsMtx(obsMtx), processNoiseCov(processNoiseCov), obsNoiseCov(obsNoiseCov), initEstimatedErrCov(estimatErrCov),
      stateDim(obsMtx.rows()), obsDim(processMatrix.rows()), dt(dt), initialized(false),
      identityMtx(obsDim, obsDim), x_hat(obsDim), x_hat_new(obsDim)

donkarlo

@staticmethod
    def plotNew(gpsNormalTestTimeAbnValsList:list,lidarNormalTestTimeAbnValsList:list):
        matplotlib.use("Qt5Agg")
        plotColsNum = max (len(gpsNormalTestTimeAbnValsList),len(lidarNormalTestTimeAbnValsList))
        fig, axes = plt.subplots(plotColsNum, 2)

        for plotRowCounter in range(plotColsNum):
            axes[plotRowCounter][0].plot(gpsNormalTestTimeAbnValsList[plotRowCounter][1][:,0]
                                         ,gpsNormalTestTimeAbnValsList[plotRowCounter][1][:,1]
                                         ,label = gpsNormalTestTimeAbnValsList[plotRowCounter][2]

donkarlo

header: 
  seq: 85173
  stamp: 
    secs: 6568
    nsecs: 392000000
  frame_id: "uav2/gps_origin"
child_frame_id: "uav2/fcu"
pose: 
  pose: 
    position: 

donkarlo

# =======================================
# topic:           /uav1/odometry/odom_gps
# msg_count:       1
# timestamp (sec): 6568.340000000
---
header: 
  seq: 85415
  stamp: 
    secs: 6568
    nsecs: 336000000

donkarlo

import matplotlib

# test scenario abn compute
import numpy as np
from matplotlib import pyplot as plt
import PyQt5

class PlotAll:

    def __init__(self):

donkarlo

# =======================================
# topic:           /uav2/rplidar/scan
# msg_count:       9
# timestamp (sec): 6882.076000000
---
header: 
  seq: 45350
  stamp: 
    secs: 6882
    nsecs:  76000000

donkarlo

    mpl.use("TkAgg")
    plt.ion()
    fig, axes = plt.subplots(2, 1)
    line1, = axes[0].plot(np.random.randn(100))
    plt.show(block=False)


    def updateGpsPlot(robotSpecificGpsTimeRows):
        line1.set_xdata( robotSpecificGpsTimeRows[:, 1])
        line1.set_ydata( robotSpecificGpsTimeRows[:, 2])

donkarlo

# test scenario abn compute
    mpl.use("TkAgg")
    plt.ion()
    fig, axes = plt.subplots(2, 1)
    line1, = axes[0].plot(np.random.randn(100))
    plt.show(block=False)


    def updateGpsPlot(robotSpecificGpsTimeRows):
        line1.set_xdata( robotSpecificGpsTimeRows[:, 1])

donkarlo

mpl.use("TkAgg")
    plt.ion()
    fig, axes = plt.subplots(2, 1)
    line1, = axes[0].plot(np.random.randn(100))
    plt.show()



    def updateGpsPlot(robotSpecificGpsTimeRows):
        line1.set_xdata( robotSpecificGpsTimeRows[:, 1])