精华内容
下载资源
问答
  • 卡尔曼滤波预测轨迹,非常有用,从纳什均衡问题的求解现状和粒子群算法的发展出发,对纳什均衡问题进行分析,并且使用粒子群算法模拟纳什均衡的博弈过程。最终使用Matlab软件实现,最后简单的改进了一下算法的初值和...
  • 卡尔曼滤波处理轨迹

    千次阅读 2020-08-20 20:22:13
    卡尔曼滤波这个词老是听到,一直也没有耐心看,最近准备看看轨迹挖掘相关的东西,第一步轨迹处理中卡尔曼滤波就又出现了,终于耐着性子研究了两天(一看一堆矩阵就脑壳痛),期间网上找了不少代码和博客,算是大概...

    卡尔曼滤波这个词老是听到,一直也没有耐心看,最近准备看看轨迹挖掘相关的东西,第一步轨迹处理中卡尔曼滤波就又出现了,终于耐着性子研究了两天(一看一堆矩阵就脑壳痛),期间网上找了不少代码和博客,算是大概明白了,还自己改了一版代码,做个小笔记

    卡尔曼滤波这个东西,我理解下来就是一方面通过理论模型算出一个预测值,另一方面通过测量手段测出一个测量值,然后通过加权平均得出一个估计值,使得这个估计值最接近真实值。

    原理的详细推导可以详细见卡尔曼滤波:从入门到精通Understanding the Basis of the Kalman Filter via a Simple and Intuitive Derivation [Lecture Notes]

    这里先记一下算法步骤
    X^k,Z^kX^kXk \hat X_k^-代表预测值, \hat Z_k代表测量值,\hat X_k代表估计值, X_k代表真实值

    • step1

    用t-1时刻的估计值通过理论模型得到t时刻的预测值
    X^k=Fk1X^k1+Bk1uk1 \hat X_k^- = F_{k-1}\hat X_{k-1}^-+B_{k-1}u_{k-1}
    更新此时的真实vs预测误差的协方差矩阵
    Pk=Fk1Pk1Fk1T+Q P_k^-= F_{k-1}P_{k-1} F_{k-1}^T + Q

    • step2

    计算卡尔曼增益
    Kk=PkHT(HPk1HT+R)1 K_k=P_k^-H^T(HP_k^-1H^T+R)^{-1}
    融合预测值测量值,更新t时刻的估计值
    X^k=X^k+Kk(Z^kHX^k) \hat X_k=\hat X_k^-+K_k(\hat Z_k-H\hat X_k^-)

    • step3

    更新此时的真实vs估计误差的协方差矩阵
    Pk=(IKkH)Pk P_k=(I-K_kH)P_k^-

    下面简单理解一下其中关键步骤是两个协方差矩阵的更新
    第一个真实vs预测误差的协方差矩阵
    构建的理论模型为
    Xk=Fk1Xk1+Bkuk+wk,wk,0Q X_k=F_{k-1}X_{k-1}+B_ku_k+w_k,w_k代表高斯噪声,均值为0,协方差矩阵为Q
    在执行更新预测值时,实际是 X^t=FtX^t1+Btut\hat X_t^-=F_t\hat X_{t-1}+B_tu_t
    则根据矩阵性质,Pk=Fk1Pk1Fk1TP_k^-=F_{k-1}P_{k-1}F_{k-1}^T,同时还要考虑wtw_t ,
    所以Pk=Fk1Pk1Fk1T+QP_k^-=F_{k-1}P_{k-1}F_{k-1}^T + Q

    可以看到预测更新实际上相当于对不确定性做“加法”:将当前状态转换到下一时刻(并增加一定不确定性即AtA_t的影响),再把外界的干扰(建模因素之外的影响,比如突然一阵风)叠加上去(又增加了一点不确定性即QQ)。

    图1 真实vs预测误差的协方差矩阵更新,图片来自[1]

    另一个是真实vs估计误差的协方差矩阵
    在这里插入图片描述

    图2 预测值分布和测量值分布的融合,图片来自 [2]

    在一维这个融合,就是两个高斯分布的乘法,而两个高斯分布的乘积仍然是高斯分布
    以下公式均来自 [2]
    在这里插入图片描述
    在这里插入图片描述
    则以上两个高斯分布做乘法后融合成的高斯分布为,如图2所示,绿色即为新的高斯分布
    在这里插入图片描述
    引入HH矩阵和高维后,就出来以下的一般形式
    在这里插入图片描述
    在这里插入图片描述
    KkK_k(卡尔曼增益)代表测量值的权重,KkK_k越大,代表测量值在估计值计算中比重越大


    kalman滤波处理轨迹实例代码,代码是根据 [3] 的基础上改的,改动的地方是状态变量XX加入了二维坐标和速度。

    # -*- coding: utf-8 -*-
    __author__ = 'fff_zrx'
    import matplotlib.pyplot as plt
    import pandas as pd
    import numpy as np
    from decimal import Decimal
    import geopandas
    from shapely import geometry
    import pyproj
    import re
    
    class KalmanFilter(object):
        def __init__(self,F,H,gps_var,pre_var):
            self.F =F # 预测时的矩阵
            self.H = H # 测量时的矩阵
            self.n=self.F.shape[0]
            self.Q = np.zeros((self.n,self.n))
            self.Q[2,2]=pre_var
            self.Q[3,3]=pre_var
            self.R = np.zeros((n,n))
            self.R[0,0]=gps_var
            self.R[1,1]=gps_var
            self.R[2,2]=gps_var
            self.R[3,3]=gps_var
            self.P = np.eye(self.n)
            self.B = np.zeros((self.n, 1))
            self.state=0
    
        #第一次传入时设置观测值为初始估计值
        def set_state(self,x,y,time_stamp):
            self.X = np.zeros((self.n, 1))
            self.speed_x=0
            self.speed_y=0
            self.X=np.array([[x],[y],[self.speed_x],[self.speed_y]])
            self.pre_X=self.X
            self.time_stamp=time_stamp
            self.duration=0
    
        def process(self,x,y,time_stamp):
            if self.state==0:
                self.set_state(x,y,time_stamp)
                self.state=1
                return x,y
    
            self.duration=(time_stamp-self.time_stamp).seconds
            self.time_stamp=time_stamp
            self.Z=np.array([[x],[y],[self.speed_x],[self.speed_y]])
            #更新时长
            self.F[0,2]=self.duration
            self.F[1,3]=self.duration
            self.predict()
            self.update()
            return self.X[0,0],self.X[1,0]
    
    
        # 预测
        def predict(self, u = 0):
            # 实现公式x(k|k-1)=F(k-1)x(k-1)+B(k-1)u(k-1)
            # np.dot(F,x)用于实现矩阵乘法
            self.X = np.dot(self.F, self.X) + np.dot(self.B, u)
            # 实现公式P(k|k-1)=F(k-1)P(k-1)F(k-1)^T+Q(k-1)
            self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
    
    
        # 状态更新,使用观测校正预测
        def update(self):
            # 新息公式y(k)=z(k)-H(k)x(k|k-1)
            y = self.Z - np.dot(self.H, self.X)
            # 新息的协方差S(k)=H(k)P(k|k-1)H(k)^T+R(k)
            S = self.R + np.dot(self.H, np.dot(self.P, self.H.T))
            # 卡尔曼增益K=P(k|k-1)H(k)^TS(k)^-1
            # linalg.inv(S)用于求S矩阵的逆
            K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
            # 状态更新,实现x(k)=x(k|k-1)+Ky(k)
            self.X = self.X + np.dot(K, y)
            #计算速度
            self.speed_x=(self.X[0,0]-self.pre_X[0,0])/self.duration
            self.speed_y=(self.X[1,0]-self.pre_X[1,0])/self.duration
            self.pre_X=self.X
            print(self.X)
            # 定义单位阵
            I = np.eye(self.n)
            # 估计值vs真实值 协方差更新
            # P(k)=[I-KH(k)]P(k|k-1)
            self.P = np.dot(I - np.dot(K, self.H), self.P)
    
    
    data=pd.read_table(r'E:\data\Taxi数据\T-drive Taxi Trajectories\release\taxi_log_2008_by_id\12.txt',delimiter=',',header=None)
    data.columns=['id','time','lon','lat']
    data['time']=pd.to_datetime(data['time'])
    data=data.sort_values(by='time')
    data=data.reset_index()
    data.columns=['label','id','time','lon','lat']
    data=data.drop_duplicates(subset="time")
    data['lon']= data['lon'].astype(float)
    data['lat']= data['lat'].astype(float)
    
    # -------中位数法-------
    n=4
    data1=data[['lon','lat']].rolling(n,min_periods=1,axis=0).median()
    data=pd.concat([data[['label','id','time']],data1],axis=1)
    data['geometry']=data.apply(lambda x: geometry.Point(x.lon,x.lat),axis=1)
    data=geopandas.GeoDataFrame(data)
    data.crs = pyproj.CRS.from_user_input('EPSG:4326')
    data=data.to_crs(crs="EPSG:2385")
    data['geometry']=data['geometry'].astype(str)
    data['x']=data['geometry'].apply(lambda x: float(re.findall(r'POINT \((.*?) ',x)[0]))
    data['y']=data['geometry'].apply(lambda x: float(re.findall(r'\d (.*?)\)',x)[0]))
    data.to_csv(r'C:\Users\fff507\Desktop\before.csv',index=False)
    
    
    # ------卡尔曼滤波------
    # 状态变量的个数,x,y,speed_x,speed_y
    n=4
    F = np.eye(n)
    H = np.eye(n)
    # 速度噪声的方差
    pre_var=15**2
    # 坐标测量噪声的方差
    gps_var=100**2
    gps_kalman=KalmanFilter(F=F,H=H,gps_var=gps_var,pre_var=pre_var)
    lon_list=[]
    lat_list=[]
    for index,row in data.iterrows():
        lon,lat=gps_kalman.process(x=row['x'],y=row['y'],time_stamp=row['time'])
        lon_list.append(lon)
        lat_list.append(lat)
    print(lon_list)
    data['new_lon']=lon_list
    data['new_lat']=lat_list
    data['geometry']=data.apply(lambda x:geometry.Point(x.new_lon,x.new_lat),axis=1)
    data=geopandas.GeoDataFrame(data)
    data.crs = pyproj.CRS.from_user_input('EPSG:2385')
    data=data.to_crs(crs="EPSG:4326")
    data['geometry']=data['geometry'].astype(str)
    data['new_lon']=data['geometry'].apply(lambda x: float(re.findall(r'POINT \((.*?) ',x)[0]))
    data['new_lat']=data['geometry'].apply(lambda x: float(re.findall(r'\d (.*?)\)',x)[0]))
    fig= plt.figure(figsize=(8,4),dpi=200)
    ax1 = fig.add_subplot(111)
    ax1.plot(data['lon'],data['lat'],'-*',label='before')
    ax1.plot(data['new_lon'],data['new_lat'],'-o',label='after',alpha=0.5)
    plt.show()
    data.to_csv(r'C:\Users\fff507\Desktop\after.csv',index=False)
    

    在这里插入图片描述

    参考资料:

    [1]卡尔曼滤波:从入门到精通
    [2]Understanding the Basis of the Kalman Filter via a Simple and Intuitive Derivation [Lecture Notes] R. Faragher. Signal Processing Magazine, IEEE , vol.29, no.5, pp.128-132, Sept. 2012 doi: 10.1109/MSP.2012.2203621
    [3]卡尔曼滤波理解与实现

    展开全文
  • 1 卡尔曼滤波是什么 卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声,观测值不准确,卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导,...

    一、简介

    1 卡尔曼滤波是什么
    卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声,观测值不准确,卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导,会让人很头疼,难以把握其中的主线和思想。所以我参考了国外一位学者的文章,讲述卡尔曼滤波的工作原理,然后编写了一个基于OpenCV的小程序给大家做一下说明。

    2 卡尔曼滤波能做什么
    假设我们手头有一辆DIY的移动小车。这辆车的外形是这样的:
    在这里插入图片描述
    这辆车可以在荒野移动,为了便于对它进行控制,需要知道它的位置以及移动速度。所以,建立一个向量,用来存储小车的位置和速度
    在这里插入图片描述
    其实,一个系统的状态有很多,选择最关心的状态来建立这个状态向量是很重要的。例如,状态还有水库里面水位的高低、炼钢厂高炉内的温度、平板电脑上面指尖触碰屏幕的位置等等这些需要持续跟踪的物理量。好了,回归到正题,小车上面安装了GPS传感器,这个传感器的精度是10米。但是如果小车行驶的荒野上面有河流和悬崖的话,10米的范围就太大,很容易掉进去进而无法继续工作。所以,单纯靠GPS的定位是无法满足需求的。另外,如果有人说小车本身接收操控着发送的运动指令,根据车轮所转动过的圈数时能够知道它走了多远,但是方向未知,并且在路上小车打滑车轮空转的现象绝对是不可避免。所以,GPS以及车轮上面电机的码盘等传感器是间接地为我们提供了小车的信息,这些信息包含了很多的和不确定性。如果将所有这些信息综合起来,是否能够通过计算得到我们更想要的准确信息呢?答案是可以的!
    在这里插入图片描述
    3 卡尔曼滤波的工作原理
    1.先验状态估计
    以之前我们创建的状态变量为例,
    在这里插入图片描述
    下图表示的是一个状态空间图,为了研究方便,假如小车在一条绝对笔直的线路上面行驶,其位置和速度的方向是确定的,不确定的是大小。
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    二、源代码

    clear;
    clc;
    %采样点的个数
    N=228;
    %测试数据:纬度
    latitude=load('C:\Users\lenovo\Desktop\基于MATLAB的运动轨迹预测,卡尔曼滤波实现\latitude.txt');
    %真实维度值
    lat=latitude;
    %卡尔曼滤波处理的状态,即估计值
    lat_kf=zeros(1,N);
    %测报值
    lat_z=zeros(1,N);
    P=zeros(1,N);
    %初始纬度值
    lat(1)=29.8131;
    %初始值的协方差
    P(1)=0.09;
    %初始测报值
    lat_z(1)=29.8027;
    %初始估计状态。假设和初始测报值相同
    lat_kf(1)=lat_z(1);
    %噪声方差
    %系统噪声方差
    Q=0.1;
    %测量噪声方差
    R=0.001;
    %方差决定噪声大小
    W=sqrt(Q)*randn(1,N);
    V=sqrt(R)*randn(1,N);
    %系统矩阵
    F=1;
    G=1;
    H=1;
    %本系统状态为1维
    I=eye(1);
    %模拟纬度测报,并滤波
    for k=2:N
        %随时间推移,飞行纬度逐渐变化
        %k时刻的真是纬度值是测报仪器不知道的,测报值可能是无限接近于真实值,但并不是真实值
        %lat(k)=F*lat(k-1)+G*W(k-1);
        %纬度在k时刻的测报值
        lat_z(k)=H*lat(k)+V(k);
        %kalman滤波
        %有了k时刻的测报值lat_z(k)和k-1时刻的状态,那么就可以进行滤波了
        %状态预测
        lat_pre=F*lat_kf(k-1);
        %协方差预测
        P_pre=F*P(k-1)*F'+Q;
        %计算卡尔曼增益
        Kg=P_pre*inv(H*P_pre*H'+R);
        %新息
        e=lat_z(k)-H*lat_pre;
        %状态更新
        lat_kf(k)=lat_pre+Kg*e;
        %协方差更新
        P(k)=(I-Kg*H)*P_pre;
    end
    %计算误差
    %测量值与真实值之间的偏差
    Err_Messure=zeros(1,N);
    %kalman估计与真实值之间的偏差
    Err_Kalman=zeros(1,N);
    for k=1:N
        Err_Messure(k)=abs(lat_z(k)-lat(k));
        Err_Kalman(k)=abs(lat_kf(k)-lat(k));
    end
    t=1:N;
    

    三、运行结果

    在这里插入图片描述
    在这里插入图片描述

    四、备注

    版本:2014a

    展开全文
  • 基于matlab elman神经网络的房价预测 二、源代码 % elm_stockpredict.m %% 清除工作空间中的变量和图形 clear,clc close all %% 1.加载337期上证指数开盘价格 load matlab.mat whos rng(1) %% ARMA模型 z=iddata...

    一、简介

    1 卡尔曼滤波是什么
    卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声,观测值不准确,卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导,会让人很头疼,难以把握其中的主线和思想。所以我参考了国外一位学者的文章,讲述卡尔曼滤波的工作原理,然后编写了一个基于OpenCV的小程序给大家做一下说明。

    2 卡尔曼滤波能做什么
    假设我们手头有一辆DIY的移动小车。这辆车的外形是这样的:
    在这里插入图片描述
    这辆车可以在荒野移动,为了便于对它进行控制,需要知道它的位置以及移动速度。所以,建立一个向量,用来存储小车的位置和速度
    在这里插入图片描述
    其实,一个系统的状态有很多,选择最关心的状态来建立这个状态向量是很重要的。例如,状态还有水库里面水位的高低、炼钢厂高炉内的温度、平板电脑上面指尖触碰屏幕的位置等等这些需要持续跟踪的物理量。好了,回归到正题,小车上面安装了GPS传感器,这个传感器的精度是10米。但是如果小车行驶的荒野上面有河流和悬崖的话,10米的范围就太大,很容易掉进去进而无法继续工作。所以,单纯靠GPS的定位是无法满足需求的。另外,如果有人说小车本身接收操控着发送的运动指令,根据车轮所转动过的圈数时能够知道它走了多远,但是方向未知,并且在路上小车打滑车轮空转的现象绝对是不可避免。所以,GPS以及车轮上面电机的码盘等传感器是间接地为我们提供了小车的信息,这些信息包含了很多的和不确定性。如果将所有这些信息综合起来,是否能够通过计算得到我们更想要的准确信息呢?答案是可以的!
    在这里插入图片描述
    3 卡尔曼滤波的工作原理
    1.先验状态估计
    以之前我们创建的状态变量为例,
    在这里插入图片描述
    下图表示的是一个状态空间图,为了研究方便,假如小车在一条绝对笔直的线路上面行驶,其位置和速度的方向是确定的,不确定的是大小。
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    二、源代码

    clear;
    clc;
    %采样点的个数
    N=228;
    %测试数据:纬度
    latitude=load('C:\Users\lenovo\Desktop\基于MATLAB的运动轨迹预测,卡尔曼滤波实现\latitude.txt');
    %真实维度值
    lat=latitude;
    %卡尔曼滤波处理的状态,即估计值
    lat_kf=zeros(1,N);
    %测报值
    lat_z=zeros(1,N);
    P=zeros(1,N);
    %初始纬度值
    lat(1)=29.8131;
    %初始值的协方差
    P(1)=0.09;
    %初始测报值
    lat_z(1)=29.8027;
    %初始估计状态。假设和初始测报值相同
    lat_kf(1)=lat_z(1);
    %噪声方差
    %系统噪声方差
    Q=0.1;
    %测量噪声方差
    R=0.001;
    %方差决定噪声大小
    W=sqrt(Q)*randn(1,N);
    V=sqrt(R)*randn(1,N);
    %系统矩阵
    F=1;
    G=1;
    H=1;
    %本系统状态为1维
    I=eye(1);
    %模拟纬度测报,并滤波
    for k=2:N
        %随时间推移,飞行纬度逐渐变化
        %k时刻的真是纬度值是测报仪器不知道的,测报值可能是无限接近于真实值,但并不是真实值
        %lat(k)=F*lat(k-1)+G*W(k-1);
        %纬度在k时刻的测报值
        lat_z(k)=H*lat(k)+V(k);
        %kalman滤波
        %有了k时刻的测报值lat_z(k)和k-1时刻的状态,那么就可以进行滤波了
        %状态预测
        lat_pre=F*lat_kf(k-1);
        %协方差预测
        P_pre=F*P(k-1)*F'+Q;
        %计算卡尔曼增益
        Kg=P_pre*inv(H*P_pre*H'+R);
        %新息
        e=lat_z(k)-H*lat_pre;
        %状态更新
        lat_kf(k)=lat_pre+Kg*e;
        %协方差更新
        P(k)=(I-Kg*H)*P_pre;
    end
    %计算误差
    %测量值与真实值之间的偏差
    Err_Messure=zeros(1,N);
    %kalman估计与真实值之间的偏差
    Err_Kalman=zeros(1,N);
    for k=1:N
        Err_Messure(k)=abs(lat_z(k)-lat(k));
        Err_Kalman(k)=abs(lat_kf(k)-lat(k));
    end
    t=1:N;
    

    三、运行结果

    在这里插入图片描述
    在这里插入图片描述

    四、备注

    版本:2014a

    展开全文
  • 1. 理论 最近学习卡尔曼滤波,由于卡尔曼滤波的公式...2. 无人机轨迹预测与仿真 (卡尔曼滤波) 2.1 卡尔曼滤波理解 总体来说仿真卡尔曼滤波就是在已知状态方程的情况下得到各矩阵的值,先算出加上高斯噪声的测量值,得

    1. 理论

    最近学习卡尔曼滤波,由于卡尔曼滤波的公式都很简单,这里仅仅是将最终公式贴在这里,他是由贝叶斯滤波在假设各变量为高斯分布且相互独立的情况下推出来的,公式如下:
    预测公式
    更新公式
    以上公式是卡尔曼滤波(KF)的公式,对于扩展卡尔曼滤波无非就是状态转移矩阵的方程是非线性,我们仅仅针对非线性局部线性化,对其求雅可比矩阵,之后按照卡尔曼滤波公式进行计算即可。

    2. 无人机轨迹预测与仿真 (卡尔曼滤波)

    2.1 卡尔曼滤波理解

    总体来说仿真卡尔曼滤波就是在已知状态方程的情况下得到各矩阵的值,先算出加上高斯噪声的测量值,得到测量值以后,就可以根据卡尔曼公式,首先初始化,之后进行预测,得到这一时刻的预测值。预测之后得到这一时刻的更新值,也就是后验估计,之后依次迭代,最终画出来波形。
    实际上,真正场景下的卡尔曼滤波,观测值可以直接通过传感器直接获取。预测值根据传感器与状态方程间接得到。
    在PX4源码中就是根据IMU的数据间接计算去预测我们想要的姿态位置等状态,同时根据其他的一些传感器比如磁罗盘,GPS,外置视觉等或者理论推导得到我们的观测值,之后通过扩展卡尔曼滤波得到我们想要的状态。具体的解释请参照官方文档PX4中扩展卡尔曼滤波EKF的应用

    2.2 轨迹仿真

    (使用matlab2019进行仿真)

    %%%扩展卡尔曼滤波仿真
    close all;
    clear;
    clc;
    
    rng(1000)%产生随机种子,以后rand会用到
    
    
    %%  参数
    Delta_t = 0.1; %采样时间,多传感器的时候使用短板采样时间
    t_max = 100; %总的采样时间
    k_max = t_max / Delta_t;%卡尔曼滤波一共递归多少次
    
    V_c = 20; %车的初始速度
    omega_c = 0; %车的初始角速度
    H = [1 0 0; 0 1 0; 0 0 1];%观测与输入的矩阵,这里看成线性矩阵
    
    Q = [Delta_t 0 0; 0 Delta_t 0; 0 0 Delta_t/180];%预测噪声的协方差矩阵
    R = [100 0 0; 0 100 0; 0 0 0.01];%观测噪声的协方差矩阵
    
    
    %%  初始化
    m_0 = zeros(3, 1);%x_0的期望
    P_0 = [1e6 0 0; 0 1e6 0; 0 0 pi^2];%x_0的协方差
    
    x_0 = mvnrnd(m_0, P_0)'; %产生多元正态随机数 x_0是预测的三个变量的初始值随机生成的
    
    
    %%  仿真数据初始化
    xSequence = zeros(3, k_max+1);%预测的初始序列
    ySequence = zeros(3, k_max+1);%观测的初始序列
    x_hatSequence = zeros(3, k_max+1);%后验的初始序列
    k_indexC = 1; %用于数组下标补偿
    
    kSequence = 0: k_max;
    for k = kSequence
    %%  得到测量值
        if k == 0
            xSequence(:, k+k_indexC) = x_0;
        else % k > 0 
            xSequence(1, k+k_indexC) = xSequence(1, k-1+k_indexC) + V_c * Delta_t * cos(xSequence(3, k-1+k_indexC));
            xSequence(2, k+k_indexC) = xSequence(2, k-1+k_indexC) + V_c * Delta_t * sin(xSequence(3, k-1+k_indexC));
            xSequence(3, k+k_indexC) = xSequence(3, k-1+k_indexC) + omega_c * Delta_t;
            xSequence(:, k+k_indexC) = xSequence(:, k+k_indexC) + mvnrnd(zeros(3, 1), Q)'; % Add process/dynamicial noise
        end
        %上面的操作主要是得到这里的测量值 xSequence是预测值 ySequence是得到的虚拟的测量值,实际中就是传感器的采样值(GPS)
        ySequence(:, k+k_indexC) = H * xSequence(:, k+k_indexC) + mvnrnd(zeros(3, 1), R)';
        
    %%	状态估计
    %%	预测步
        if k == 0
            m_k_prior = m_0;%初始期望
            P_k_prior = P_0;%初始方差
        else % k > 0%首先得到预测的雅可比矩阵 主要针对非线性
            F_k_x = [1 0 -V_c*Delta_t*sin(m_kminus1_posterior(3)); 0 1 V_c*Delta_t*cos(m_kminus1_posterior(3)); 0 0 1]; % Jacobian matrix of f_k
            %依次 得到预测的点的期望 这里期望就代表状态,因为这是卡尔曼滤波 
            m_k_prior(1) = m_kminus1_posterior(1) + V_c * Delta_t * cos(m_kminus1_posterior(3));
            m_k_prior(2) = m_kminus1_posterior(2) + V_c * Delta_t * sin(m_kminus1_posterior(3));
            m_k_prior(3) = m_kminus1_posterior(3) + omega_c * Delta_t;
            %依次 得到预测的点的方差 这里期望就代表状态,因为这是卡尔曼滤波 
            P_k_prior = F_k_x * P_kminus1_posterior * F_k_x' + Q;
        end
        
    %% 更新步
        %测量残差协方差
        S_k = H * P_k_prior * H' + R;
        %卡尔曼增益
        K_k = P_k_prior * H' / S_k;
        
        m_k_posterior = m_k_prior + K_k * (ySequence(:, k+k_indexC) - H * m_k_prior);%更新的期望 最终状态
        P_k_posterior = P_k_prior - K_k * S_k * K_k';%更新的协方差
        P_k_posterior = (P_k_posterior + P_k_posterior')/2; % 
        x_hatSequence(:, k+k_indexC) = m_k_posterior; % 
        %用于递归
        m_kminus1_posterior = m_k_posterior;
        P_kminus1_posterior = P_k_posterior;
    end
    
    
    %% 图形化表示
    tSequence = Delta_t * kSequence;
    
    figure,
    plot(tSequence, xSequence(1, :), tSequence, x_hatSequence(1, :), 'LineWidth', 0.5)
    xlabel('Time (sec.)')
    ylabel('Locations (m)')
    title('x^{(1)}-axis')
    legend('x^{(1)}', 'Estimate of x^{(1)}')
    grid on;
    
    figure,
    plot(tSequence, xSequence(2, :), tSequence, x_hatSequence(2, :), 'LineWidth', 0.5)
    xlabel('Time (sec.)')
    ylabel('Locations (m)')
    title('x^{(2)}-axis')
    legend('x^{(2)}', 'Estimate of x^{(2)}')
    grid on;
    
    figure,
    plot(tSequence, xSequence(3, :), tSequence, x_hatSequence(3, :), 'LineWidth', 0.5)
    xlabel('Time (sec.)')
    ylabel('Angle (rad)')
    title('\theta')
    legend('\theta', 'Estimate of \theta')
    grid on;
    
    figure,
    plot(xSequence(1, :), xSequence(2, :), x_hatSequence(1, :), x_hatSequence(2, :), 'LineWidth', 0.5)
    xlabel('x^{(1)}-axis (m)')
    ylabel('x^{(2)}-axis (m)')
    legend('Trajectory', 'Estimate')
    grid on;
    

    3. 仿真结果

    3.1 x轴原始值与预测值

    在这里插入图片描述
    可以看出预测结果基本一致,当然这也是建立在我们仿真阶段,因为仿真我们对于位移的预测非常准确,其预测的方差很小,观测的方差很大,导致最终结果很相信预测,所以主观看起来非常准确。

    3.2 y轴原始值与预测值

    在这里插入图片描述
    可以看出预测结果基本一致。

    3.3 转向角原始值与预测值

    在这里插入图片描述
    角度的预测结果稍微差一些,主要是因为不仅预测的方差很小,观测的方差也很小,这样一折中,导致误差大了起来。不过整体结果还不错,趋势上差别不大。

    展开全文
  • 看了一些文献,在轨迹预测方面,卡尔曼滤波多数是做单步预测,但实际生活中,单步预测可能不够,有没有路过的老铁知道基于卡尔曼滤波的多步轨迹预测的案例
  • 1 卡尔曼滤波是什么 卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声,观测值不准确,卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导,...
  • 卡尔曼滤波

    2013-12-12 18:53:28
    卡尔曼滤波预测轨迹程序,Matlab程序可以帮助你学习卡尔曼滤波
  • 运动物体的轨迹预测,分别使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及数据拟合方法实现。本例代码仅含卡尔曼滤波部分代码。 本例仅为本人在研究轨迹预测问题时为理解算法原理所写,针对具体问题请自行斟酌...
  • 运动物体的轨迹预测,分别使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及数据拟合方法实现。本例代码仅含扩展卡尔曼滤波部分代码。 本例仅为本人在研究轨迹预测问题时为理解算法原理所写,针对具体问题请自行...
  • 运动物体的轨迹预测,分别使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及数据拟合方法实现。本例代码仅含无迹卡尔曼滤波部分代码。 本例仅为本人在研究轨迹预测问题时为理解算法原理所写,针对具体问题请自行...
  • 用C++实现了卡尔曼滤波对飞行轨迹预测算法,并附有测试数据
  • 一些概率论的知识基础卡尔曼滤波完整推导状态估计计算卡尔曼增益预测和更新 卡尔曼滤波部分我打算分三节(三次博客的内容): 卡尔曼滤波与行人状态估计 扩展卡尔曼滤波(EKF)与传感器融合 过程模型,无损卡尔曼...
  • 对高速运动目标采用基于kalman filter进行预测。基于matlab的实现,来进行运动目标的轨迹预测。有卡尔曼算法,扩展卡尔曼滤波,数据拟合方法。
  • 最优估计下/第一次课程实习/抛体轨迹预测(拓展的卡尔曼滤波)/内含老师给的数据/Matlab完整代码/
  • 应用卡尔曼滤波对机器人行人跟随--路径轨迹预测 ​ 由于卡尔曼滤波能够依靠变量上一个时刻的值和当前时刻的值对下一时刻的值进行预测,我们如果依据前几个时刻的跟随目标轨迹坐标点,对下一个时刻的目标轨迹坐标点...
  • 利用协方差矩阵可实现多种特征的巧妙融合,采用基于遗忘因子的加权搜索策略,可以削弱窗口内相似目标的干扰,利用卡尔曼滤波预测目标运动轨迹并判断目标是否被严重遮挡,使遮挡消失后目标仍能被重新捕获。实验结果表明,...
  • 卡尔曼是匈牙利数学家,Kalman滤波器源于其博士毕业了论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》(线性滤波预测问题的新方法)。
  • 关于卡尔曼滤波跟踪算法的理解文章实在太多,绝...而此篇博客旨在从另外角度去逐步实现卡尔曼滤波在三维轨迹预测上来,假设读者能够理解卡尔曼的5个公式(2个predict,3个update/correction),不懂可以参考文后的...
  • 斜抛运动(网球,羽毛球)等轨迹与落点预测需要用到扩展卡尔曼滤波 这里记录一下自己学习所参考的网站 以及一些总结 直观理解 一开始根本知道卡尔曼滤波是干什么,所以这两个介绍给了一些直观的解释 1.作者截取于某...
  • 根据道路交通监控视频的特点,采用压缩跟踪(CT)算法进行...为了解决这一问题,提出使用卡尔曼滤波对车辆的轨迹进行预测。丢失或被部分遮挡时,能准确而稳定地跟踪车辆,而且具有很好的实时性,满足了工程应用的需求。
  • 本文简单介绍卡尔曼滤波及其使用。 原理 卡尔曼滤波的细节可以参考下面这些,有直观解释也有数学推导。 运动目标跟踪(一)--搜索算法预测模型之KF,EKF,UKF 初学者的卡尔曼滤波——扩展卡尔曼滤波(一) 理解...
  • 因为好几个小工程项目是在假期2020年4月份-2020年6月份学习完成的,包括激光雷达和毫米波雷达融合的项目、粒子滤波定位的项目、无迹卡尔曼滤波算法、轨迹控制预测等,重新每一个都写一篇博客太多了,最近课题也有点...
  • 作为卡尔曼滤波预测车辆轨迹用到的CA,CV,CCA,CCV,CTRV,CTRA经典模型

空空如也

空空如也

1 2 3 4
收藏数 64
精华内容 25
关键字:

卡尔曼滤波预测轨迹