信息滤波器

最新推荐文章于 2023-01-09 18:49:51 发布

置顶 td092

最新推荐文章于 2023-01-09 18:49:51 发布

阅读量1.4k

点赞数 1

分类专栏：机器人

欢迎交流，有评论就更

本文链接：https://blog.csdn.net/weixin_38258767/article/details/106079189

版权

机器人专栏收录该内容

63 篇文章 8 订阅

订阅专栏

信息滤波器(IF)是卡尔曼滤波器(KF)的对偶滤波器。卡尔曼滤波器维护的变量包括状态均值 $\mu$ 和状态协方差 $\Sigma$ 。而信息矩阵维护的变量包括信息向量 $\xi$ 和信息矩阵 $\Omega$ 。

从KF到IF

在KF中我们用高斯分布来描述状态变量的分布情况:
$det(2\pi\Sigma)^{-\frac{1}{2}}exp(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu))$

而信息矩阵和信息向量，它们与均值和方差的关系为：
$\Omega=\Sigma^{-1}\\\xi=\Sigma^{-1}\mu$
对于KF，它的预测和更新方程如下：
$\bar{\mu}_t = A_t\mu_{t-1}+B_tu_t\\ \bar{\Sigma}_t = A_t\Sigma_{t-1}A^T_t+R_t\\ K_t=\bar{\Sigma}_tC^T_t(C_t\bar{\Sigma}_tC^T_t+Q_t)^{-1}\\ \mu_t = \bar{\mu}_t+K_t(z_t-C_t\bar{\mu}_t)\\ \Sigma_t=(I-K_tC_t)\bar{\Sigma}_t$
将 $\Omega$ 和 $\xi$ 代入可得，IF的预测方程为：
$\bar{\Omega}_t=(A_t\Omega^{-1}_{t-1}A^T_t+R_t)^{-1}\\ \bar{\xi}_t=\bar{\Omega}_t(A_t\Omega^{-1}_{t-1}\xi_{t-1}+B_tu_t)$
下面推导IF的更新方程：
将 $\Omega$ 和 $\xi$ 代入上面的高斯分布概率密度函数有：
$\eta exp(-\frac{1}{2}x^T\Omega x+x^T\xi)\\ \eta = \frac{exp(-\frac{1}{2}\mu^T\xi)}{det(2\pi\Omega^{-1})^{\frac{1}{2}}}$
而根据贝叶斯公式，推出后验概率：
$bel(x_i)=\eta p(z_t|x_t)\bar{bel}(x_t)\\ =\eta exp(-\frac{1}{2}(z_t-C_tx_t)^TQ_t^{-1}(x_t-C_tx_t)) exp(-\frac{1}{2}(x_t-\bar{\mu}_t)^T\bar{\Sigma}^{-1}(x_t-\bar{\mu}_t))\\ = \eta exp(\frac{1}{2}x^T_t[C_t^TQ^{-1}_tC_t+\bar{\Omega}_t]x_t+x^T_t[C^T_tQ^{-1}_tz_t+\bar{\xi}_t])$
所以，IF的更新方程为：
$\Omega_t=C^T_tQ^{-1}_tC_t+\bar{\Omega}_t\\ \xi_t = C^T_tQ^{-1}_tz_t+\bar{\xi}_t$

IF的python实现

import numpy as np
from numpy.linalg import inv
from numpy import dot
from matplotlib import pyplot as plt
import seaborn

class BasicMovement:
    def __init__(self, maxSpeed, covariance, robotFeaturesDim):
        self.maxSpeed = maxSpeed
        self.covariance = np.atleast_2d(covariance)
        self.robotFeaturesDim = robotFeaturesDim

    #  Input the real state
    def move(self, state, covariance=None, command=None):
        move = self.__choose_command(state) if command is None else command
        noise = self.__get_noise(state, covariance)
        newState = state + move + noise
        return newState, move

    def __choose_command(self, state):  # TO CHANGE FOR 2D
        dim = self.robotFeaturesDim
        way = 1 - 2 * np.random.randint(2)
        speed = self.maxSpeed * np.random.rand()
        newCommand = np.zeros_like(state)
        newCommand[:dim] = way * speed  # TO CHANGE FOR 2D
        return newCommand

    def __get_noise(self, state, covariance):
        dim = self.robotFeaturesDim
        noise = np.zeros_like(state)
        covariance = self.covariance if covariance is None else covariance
        noise[:dim] = np.random.multivariate_normal(np.zeros(dim), covariance, 1).T
        return noise


class BasicMeasurement:
    def __init__(self, covariance, robotFeaturesDim, envFeaturesDim, measureFunction, gradMeasureFunction, detectionSize=0):
        self.covariance = np.atleast_2d(covariance)
        self.robotFeaturesDim = robotFeaturesDim
        self.envFeaturesDim = envFeaturesDim
        self.measureFunction = measureFunction
        self.gradMeasureFunction = gradMeasureFunction
        self.detectionSize = detectionSize

    #  Input the real state
    def measure(self, state):
        dim = state.shape[0]
        dimR = self.robotFeaturesDim
        dimE = self.envFeaturesDim
        rState = state[:dimR]
        envState = state[dimR:]
        nbLandmark = (dim - dimR) / dimE
        nbLandmark = int(nbLandmark)
        mes = np.zeros(nbLandmark)
        landmarkIds = np.zeros(nbLandmark).astype(int)
        j = 0
        for i, landmark in enumerate(envState.reshape((nbLandmark, dimE, 1))):
            if (np.linalg.norm(rState - landmark) < self.detectionSize) or (self.detectionSize is 0):
                mes[j] = self.measureFunction(rState, landmark)
                landmarkIds[j] = int(i)
                j += 1
        mes = mes[:j]
        landmarkIds = landmarkIds[:j]

        mes = np.array(mes)
        noise = self.__get_noise(mes)
        mes += noise  
        return mes.reshape((len(mes), 1)), landmarkIds

    def __get_noise(self, mes):
        noise = np.squeeze(np.random.multivariate_normal(np.zeros(self.envFeaturesDim), self.covariance, len(mes)))
        return noise


class EIFModel:
    def __init__(self, dimension, robotFeaturesDim, envFeaturesDim, motionModel, mesModel, covMes, muInitial):
        self.robotFeaturesDim = robotFeaturesDim
        self.envFeaturesDim = envFeaturesDim
        self.dimension = dimension

        self.H = np.eye(dimension)
        self.b = dot(muInitial.T, self.H)
        self.S = np.zeros(dimension * robotFeaturesDim).reshape((dimension, robotFeaturesDim))
        self.S[:robotFeaturesDim] = np.eye(robotFeaturesDim)
        self.invZ = inv(covMes)
        self.motionModel = motionModel
        self.mesModel = mesModel

    def update(self, measures, landmarkIds, command, U):
        self.__motion_update(command, U)
        for ldmIndex, ldmMes in zip(landmarkIds, measures):
            self.__measurement_update(ldmMes, ldmIndex)
        return self.H, self.b

    def __motion_update(self, command, U):
        previousMeanState = self.estimate()
        _, meanStateChange = self.motionModel.move(previousMeanState, command=command)
        self.H = inv(inv(self.H) + dot(dot(self.S, U), self.S.T))
        self.b = dot((previousMeanState + meanStateChange).T,  self.H)

    def __measurement_update(self, ldmMes, ldmIndex):
        mu = self.estimate()
        meanMes, gradMeanMes = self.__get_mean_measurement_params(mu, ldmIndex)

        z = np.atleast_2d(ldmMes)
        zM = np.atleast_2d(meanMes)
        C = gradMeanMes

        self.H += dot(dot(C, self.invZ),  C.T)
        self.b += dot(dot((z - zM + dot(C.T, mu)).T, self.invZ), C.T)

    def __get_mean_measurement_params(self, mu, ldmIndex):
        realIndex = self.robotFeaturesDim + ldmIndex * self.envFeaturesDim
        # import ipdb; ipdb.set_trace()
        ldmMeanState = mu[realIndex: realIndex + self.envFeaturesDim]
        rMeanState = mu[:self.robotFeaturesDim]

        meanMes = self.mesModel.measureFunction(rMeanState, ldmMeanState)
        gradMeanMes = self.mesModel.gradMeasureFunction(mu, ldmIndex)
        return meanMes, gradMeanMes

    def estimate(self, H=None, b=None):
        H = self.H if H is None else H
        b = self.b if b is None else b
        return dot(b, inv(H)).T
measureFunction = lambda rState, landmark: np.sign(landmark[0, 0] - rState[0, 0]) * np.linalg.norm(rState - landmark)

def gradMeasureFunction(state, ldmIndex):
    grad = np.zeros_like(state)
    grad[0] = -1
    grad[ldmIndex+1] = 1
    return grad


T = 1000  # 仿真步数
nbLandmark = 10 ## 路标个数
maxSpeed = 3 ## 最大速度
robotFeaturesDim = 1 ## 机器人状态维数
envFeaturesDim = 1 ## 环境状态维数
dimension = robotFeaturesDim + nbLandmark * envFeaturesDim ##总状态维数

# Detection parameters
spaceBetween = 100 ## 路标间隔
detectionSize = 15 ## 探测范围

covarianceMotion = np.eye(robotFeaturesDim) * 10  # 运动噪声
covarianceMeasurements = np.eye(envFeaturesDim) * 10  # 观测噪声

motionModel = BasicMovement(maxSpeed, covarianceMotion, robotFeaturesDim)
measurementModel = BasicMeasurement(covarianceMeasurements, robotFeaturesDim, envFeaturesDim, measureFunction, gradMeasureFunction, detectionSize)
state = np.zeros((dimension, 1))  # 机器人状态和路标位置
state[1:] = np.arange(0, nbLandmark * spaceBetween, spaceBetween).reshape(nbLandmark, 1)# 设置路标


muEIF = np.zeros_like(state)  # 初始状态
muEIF = state.copy()
muEIF[1:] += np.random.multivariate_normal([0], covarianceMeasurements, nbLandmark).reshape(nbLandmark, 1)


eif = EIFModel(dimension, robotFeaturesDim, envFeaturesDim, motionModel, measurementModel, covarianceMeasurements, muEIF)

mus_eif = np.zeros((T, dimension))##记录IF滤波值和真值
states = np.zeros((T, dimension))

mus_eif[0] = np.squeeze(muEIF)
states[0] = np.squeeze(state)

listStates = []
listCommands = []
listMeasures = []
listLandmarkIds = []
for t in range(1, T):
    state, motionCommand = motionModel.move(state)
    measures, landmarkIds = measurementModel.measure(state)
    listStates.append(state)
    listCommands.append(motionCommand)
    listMeasures.append(measures)
    listLandmarkIds.append(landmarkIds)


for t in range(1, T):
    state = listStates[t-1]
    motionCommand = listCommands[t-1]
    measures = listMeasures[t-1]
    landmarkIds = listLandmarkIds[t-1]

    H, b = eif.update(measures, landmarkIds, motionCommand, covarianceMotion)
    muEIF = eif.estimate()
    mus_eif[t] = np.squeeze(muEIF)
    
plt.figure()
plt.plot(states[:, 0])
plt.plot(mus_eif[:, 0])
plt.legend(['Real position', 'EIF estimate'])
plt.title("{0} landmarks".format(nbLandmark))
plt.show()

在这里插入图片描述

参考：
http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam06-eif.pdf
https://github.com/theevann/SLAM

td092

关注

1
点赞
踩
11

收藏

觉得还不错? 一键收藏
0
评论
信息滤波器

信息滤波器(IF)是卡尔曼滤波器(KF)的对偶滤波器。卡尔曼滤波器维护的变量包括状态均值μ\muμ和状态协方差Σ\SigmaΣ。而信息矩阵维护的变量包括信息向量ξ\xiξ和信息矩阵Ω\OmegaΩ。从KF到IF在KF中我们用高斯分布来描述状态变量的分布情况:p(x)=det(2πΣ)−12exp(−12(x−μ)TΣ−1(x−μ))p(x) = det(2\pi\Sigma)^{-\frac{1}{2}}exp(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu))p(x)=
复制链接

扫一扫