zoukankan      html  css  js  c++  java
  • 卡尔曼滤波— Constant Velocity Model

    假设你开车进入隧道,GPS信号丢失,现在我们要确定汽车在隧道内的位置。汽车的绝对速度可以通过车轮转速计算得到,汽车朝向可以通过yaw rate sensor(A yaw-rate sensor is a gyroscopic device that measures a vehicle’s angular velocity around its vertical axis. )得到,因此可以获得X轴和Y轴速度分量Vx,Vy

    首先确定状态变量,恒速度模型中取状态变量为汽车位置和速度:

    根据运动学定律(The basic idea of any motion models is that a mass cannot move arbitrarily due to inertia):

    由于GPS信号丢失,不能直接测量汽车位置,则观测模型为:

    卡尔曼滤波步骤如下图所示:

     1 # -*- coding: utf-8 -*-
     2 import numpy as np
     3 import matplotlib.pyplot as plt
     4 
     5 # Initial State x0
     6 x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T
     7 
     8 # Initial Uncertainty P0
     9 P = np.diag([1000.0, 1000.0, 1000.0, 1000.0])
    10 
    11 dt = 0.1 # Time Step between Filter Steps
    12 
    13 # Dynamic Matrix A
    14 A = np.matrix([[1.0, 0.0, dt, 0.0],
    15               [0.0, 1.0, 0.0, dt],
    16               [0.0, 0.0, 1.0, 0.0],
    17               [0.0, 0.0, 0.0, 1.0]])
    18 
    19 # Measurement Matrix
    20 # We directly measure the velocity vx and vy
    21 H = np.matrix([[0.0, 0.0, 1.0, 0.0],
    22               [0.0, 0.0, 0.0, 1.0]])
    23 
    24 # Measurement Noise Covariance
    25 ra = 10.0**2
    26 R = np.matrix([[ra, 0.0],
    27               [0.0, ra]])
    28 
    29 # Process Noise Covariance
    30 # The Position of the car can be influenced by a force (e.g. wind), which leads
    31 # to an acceleration disturbance (noise). This process noise has to be modeled
    32 # with the process noise covariance matrix Q.
    33 sv = 8.8
    34 G = np.matrix([[0.5*dt**2],
    35                [0.5*dt**2],
    36                [dt],
    37                [dt]])
    38 Q = G*G.T*sv**2
    39 
    40 I = np.eye(4)
    41 
    42 # Measurement
    43 m = 200 # 200个测量点
    44 vx= 20  # in X
    45 vy= 10  # in Y
    46 mx = np.array(vx+np.random.randn(m))
    47 my = np.array(vy+np.random.randn(m))
    48 measurements = np.vstack((mx,my))
    49 
    50 # Preallocation for Plotting
    51 xt = []
    52 yt = []
    53 
    54 
    55 # Kalman Filter
    56 for n in range(len(measurements[0])):
    57     
    58     # Time Update (Prediction)
    59     # ========================
    60     # Project the state ahead
    61     x = A*x
    62     
    63     # Project the error covariance ahead
    64     P = A*P*A.T + Q
    65 
    66     
    67     # Measurement Update (Correction)
    68     # ===============================
    69     # Compute the Kalman Gain
    70     S = H*P*H.T + R
    71     K = (P*H.T) * np.linalg.pinv(S)
    72 
    73     # Update the estimate via z
    74     Z = measurements[:,n].reshape(2,1)
    75     y = Z - (H*x)                            # Innovation or Residual
    76     x = x + (K*y)
    77     
    78     # Update the error covariance
    79     P = (I - (K*H))*P
    80 
    81     
    82     # Save states for Plotting
    83     xt.append(float(x[0]))
    84     yt.append(float(x[1]))
    85 
    86 
    87 # State Estimate: Position (x,y)
    88 fig = plt.figure(figsize=(16,16))
    89 plt.scatter(xt,yt, s=20, label='State', c='k')
    90 plt.scatter(xt[0],yt[0], s=100, label='Start', c='g')
    91 plt.scatter(xt[-1],yt[-1], s=100, label='Goal', c='r')
    92 
    93 plt.xlabel('X')
    94 plt.ylabel('Y')
    95 plt.title('Position')
    96 plt.legend(loc='best')
    97 plt.axis('equal')
    98 plt.show()

    汽车在隧道中的估计位置如下图:

     参考

    Improving IMU attitude estimates with velocity data

    https://zhuanlan.zhihu.com/p/25598462

  • 相关阅读:
    赫尔维茨公式
    从解析几何的角度分析二次型
    Struts 1 Struts 2
    记一次服务器被入侵的调查取证
    契约式设计 契约式编程 Design by contract
    lsblk df
    Linux Find Out Last System Reboot Time and Date Command 登录安全 开关机 记录 帐号审计 历史记录命令条数
    Infrastructure for container projects.
    更新文档 版本控制 多版本并发控制
    Building Microservices: Using an API Gateway
  • 原文地址:https://www.cnblogs.com/21207-iHome/p/5274819.html
Copyright © 2011-2022 走看看