kalman-filter - 验证卡尔曼滤波器的答案

问题描述

所以，我在这个数据集上应用了卡尔曼滤波器。以下是我的代码（请注意，我正在为一个人添加整个代码以在他/她的机器上重现结果）。

# Multi dimensional Kalman filter
import os
from math import *

class matrix:

    # implements basic operations of a matrix class

    def __init__(self, value):
        if isinstance(value, basestring):
            print "lol"
        self.value = value

        self.dimx = len(value)
        self.dimy = len(value[0])
        if value == [[]]:
            self.dimx = 0

    def zero(self, dimx, dimy):
        # check if valid dimensions
        if dimx < 1 or dimy < 1:
            raise ValueError, "Invalid size of matrix"
        else:
            self.dimx = dimx
            self.dimy = dimy
            self.value = [[0 for row in range(dimy)] for col in range(dimx)]

    def identity(self, dim):
        # check if valid dimension
        if dim < 1:
            raise ValueError, "Invalid size of matrix"
        else:
            self.dimx = dim
            self.dimy = dim
            self.value = [[0 for row in range(dim)] for col in range(dim)]
            for i in range(dim):
                self.value[i][i] = 1

    def show(self):
        for i in range(self.dimx):
            print self.value[i]
        print ' '

    def __add__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError, "Matrices must be of equal dimensions to add"
        else:
            # add if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] + other.value[i][j]
            return res

    def __sub__(self, other):
        # check if correct dimensions
        if self.dimx != other.dimx or self.dimy != other.dimy:
            raise ValueError, "Matrices must be of equal dimensions to subtract"
        else:
            # subtract if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, self.dimy)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[i][j] = self.value[i][j] - other.value[i][j]
            return res

    def __mul__(self, other):
        # check if correct dimensions

        if self.dimy != other.dimx:
            raise ValueError, "Matrices must be m*n and n*p to multiply"
        else:
            # subtract if correct dimensions
            res = matrix([[]])
            res.zero(self.dimx, other.dimy)
            for i in range(self.dimx):
                for j in range(other.dimy):
                    for k in range(self.dimy):
                        res.value[i][j] += self.value[i][k] * other.value[k][j]
            return res

    def transpose(self):
        # compute transpose
        res = matrix([[]])
        res.zero(self.dimy, self.dimx)
        for i in range(self.dimx):
            for j in range(self.dimy):
                res.value[j][i] = self.value[i][j]
        return res

    # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions

    def Cholesky(self, ztol=1.0e-5):
        # Computes the upper triangular Cholesky factorization of
        # a positive definite matrix.
        res = matrix([[]])
        res.zero(self.dimx, self.dimx)

        for i in range(self.dimx):
            S = sum([(res.value[k][i])**2 for k in range(i)])
            d = self.value[i][i] - S
            if abs(d) < ztol:
                res.value[i][i] = 0.0
            else:
                if d < 0.0:
                    raise ValueError, "Matrix not positive-definite"
                res.value[i][i] = sqrt(d)
            for j in range(i+1, self.dimx):
                S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
                if abs(S) < ztol:
                    S = 0.0
                res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
        return res

    def CholeskyInverse(self):
        # Computes inverse of matrix given its Cholesky upper Triangular
        # decomposition of matrix.
        res = matrix([[]])
        res.zero(self.dimx, self.dimx)

        # Backward step for inverse.
        for j in reversed(range(self.dimx)):
            tjj = self.value[j][j]
            S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
            res.value[j][j] = 1.0/tjj**2 - S/tjj
            for i in reversed(range(j)):
                res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
        return res

    def inverse(self):
        aux = self.Cholesky()
        res = aux.CholeskyInverse()
        return res

    def __repr__(self):
        return repr(self.value)


########################################

# filter function

def kalman_filter(x, P):

        # measurement update

        y = measurements - H * x

        s = H * P * H.transpose() + R
        K = P * H.transpose() * s.inverse()
        x = x + K * y
        P = (I - K * H) * P

        # prediction
        x = F * x  
        P = F * P * F.transpose()
        return x,P

files = []
x = matrix([[0.], [0.], [0.]]) # initial state (location and velocity)

P = matrix([[1000., 0.,0.], [0., 1000.,0.] , [0.,0.,1000.]]) # initial uncertainty
u = matrix([[0.], [0.]]) # external motion
F = matrix([[1., 0., 0.], [0., 1.,0.], [0.,0.,1.] ]) # next state function
H = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) # measurement function
R = matrix([[1., 0., 0.], [0.,1.,0.], [0., 0., 1.]]) # measurement uncertainty
I = matrix([[1., 0.,0.], [0., 1.,0.], [0.,0.,1.]]) # identity matrix
for i in os.listdir("/home/fatima/Downloads/thesis/HMP_Dataset/Climb_stairs"):
    if i.endswith('.txt'):
        files.append(i)






for j in range(len(files)-1):
    print "iterationh number"
    print j
    with open("/home/fatima/Downloads/thesis/HMP_Dataset/Climb_stairs/"+files[j]) as f:

        content = f.readlines()
        for e in range(len(content)-1):
            content1 = [z.strip() for z in content]
            content2 = content1[e].split(" ")
            content2[0] =-14.709 + (float(content2[0])/63)*(2*14.709);
            content2[2] =-14.709 + (float(content2[2])/63)*(2*14.709);
            content2[1] =-14.709 + (float(content2[1])/63)*(2*14.709);
            measurements =matrix([ [float(content2[0]) ], [float(content2[1])] , [float(content2[2])] ] )
            print measurements



            x,p=  kalman_filter(x, P)
            print x

卡尔曼滤波器在代码的末尾。我没有在状态矩阵的预测中添加任何噪声，因为我不确定如何。 关于数据集： 它是一个 ADL 腕戴式加速度计数据集。共有 14 个活动，加速度记录在 x、y 和 z 方向。加速度值从 0 到 63 不等。对于计算，它被归一化为 -14.7 到 +14.7。 问题： 我的问题是我是否朝着正确的方向前进。输出似乎正确吗？有什么改进吗？

标签： kalman-filter

代码看起来不错，你肯定朝着正确的方向前进！

以下是您可以检查您的实施的一些想法：

考虑您知道解决方案的简单情况，例如 H = I、P = sigma²*I 和 R = sigma'² * R. x 应该趋向于 sigma 的前一个时间步的 x 趋于零并且 x 应该趋于如果 sigma' 趋于零，则朝向测量值。或者如果 sigma = sigma'，那么卡尔曼滤波器分析应该是先前状态的平均值，并且测量值和 P 应该减少一半。您可能想为这些情况编写单元测试。
检查矩阵 P 是否始终保持对称和正定义（所有特征值都是正的）
实施备用卡尔曼滤波器更新步骤，参见文档http://modb.oce.ulg.ac.be/mediawiki/upload/Alex/AssimLecture/assim_lecture.pdf第 39 页上的公式 (28) 。本质上，您也可以将卡尔曼增益计算为：

K = inv(inv(P) + H' * inv(R) H ) H' inv(R)

其中 inv(P) 是矩阵 P 的逆矩阵。两者都应等于数值精度。

对于模型噪声，您可以在方程 P = F * P * F' + Q 中添加一个小的协方差矩阵。它通常是与单位矩阵成比例的矩阵。如果 Q 太小，经过一些迭代后测量将不再影响分析。如果 Q 太大，您对状态 x 的建模将非常嘈杂。

一些编码备注：

如果measurements和H也是RKalman_filter 函数的参数，那么您的函数将更容易移植到不同的情况。
你知道矩阵运算的 numpy 吗？它广泛支持矩阵和数组运算，并使用高度优化的库进行计算。

让我知道这是否有帮助！

kalman-filter - 验证卡尔曼滤波器的答案

问题描述

解决方案

推荐阅读