课程 5： Problem Set 2

课程 5： Problem Set 2

1.   练习： Measurement Update

2.   练习：New Variance

回顾了在测量时，比如由于测量设备存在误差，而且已知测量设备的方差时，通过测量手段预测下一个新的值（呈高斯分布）的均值和方差

一维世界中，测量更新代码

new_mean = (mean1*var2 +mean2*var1)/(var1+var2)

new_var = 1/(1/var1 + 1/var2)

移动卷积对均值和方差的影响预测：

new_mean= mean1 + mean2

new_var = var1 + var2

3.   练习：Heavytail Gaussian

4.   练习：How Many Dimensions

一维世界中，如果用2个值，位置和速度，来表示状态向量的话，到二维世界，就需要4个值来表示状态向量了。

5.   练习：State Transition Matrix

假设有一个二维空间的匀速直线运动，dt = 0.1 ,x’=x+dt * V_x, y’=y+dt * V_y状态向量为[x,y,V_x,V_y]^T

则状态转移矩阵为：

F = matrix([[1., 0, dt, 0], [0, 1., 0, dt],[0,0, 1, 0],[0, 0, 0, 1]])

6.   练习：Programming Exercise

这次的matrix 类，跟课程4中的代码里实现的一样的，上次是给定公式相关的各个矩阵等参数值，写出代码公式。这次是给定代码，要求写出符合要求的变量值。

代码跟上次的有点区别，先调用了prediction,再做的测量更新。所以课程4中过滤器打印的x,和p是预测到的下一个的值，而这次打印的x和p是对最后一次的测量值的更新。过滤器代码如下：

def filter(x, P):

   for n in range(len(measurements)):

       

       # prediction

       x = (F * x) + u

       P = F * P * F.transpose()

       

       # measurement update

       Z = matrix([measurements[n]])

       y = Z.transpose() - (H * x)

       S = H * P * H.transpose() + R

       K = P * H.transpose() * S.inverse()

       x = x + (K * y)

       P = (I - (K * H)) * P

   

   print('x= ')

   x.show()

   print('P= ')

P.show()

 

其他参数值和调用过滤器代码：

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

 

print("### 4-dimensional example###")

 

measurements = [[5., 10.], [6., 8.], [7.,6.], [8., 4.], [9., 2.], [10., 0.]]

initial_xy = [4., 12.]

 

# measurements = [[1., 4.], [6., 0.],[11., -4.], [16., -8.]]

# initial_xy = [-4., 8.]

 

# measurements = [[1., 17.], [1., 15.],[1., 13.], [1., 11.]]

# initial_xy = [1., 19.]

 

dt = 0.1

 

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

u = matrix([[0.], [0.], [0.], [0.]]) #external motion

 

#### DO NOT MODIFY ANYTHING ABOVE HERE####

#### fill this in, remember to use thematrix() function!: ####

 

#P = # initial uncertainty: 0 for positions x and y, 1000 for the twovelocities

P = matrix([[0.,   0.,   0.,   0.],

           [0.,   0.,   0.,  0.],

           [0.,   0., 1000.,  0.],

           [0.,   0.,   0., 1000.]])

#F = # next state function: generalize the 2d version to 4d

F = matrix([[1.,   0.,  dt,   0.],

           [0.,   1.,   0.,  dt],

            [0.,  0.,   1.,   0.],

           [0.,   0.,   0.,  1.]])

#H = # measurement function: reflect the fact that we observe x and y but notthe two velocities

H = matrix([[1., 0., 0., 0],

           [0., 1., 0., 0]])

#R = # measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal

R = matrix([[0.1, 0.], [0., 0.1]])

#I = # 4d identity matrix

I = matrix([[1.,   0.,  0.,   0.],

           [0.,   1,    0.,  0.],

           [0.,   0.,   1.,  0.],

           [0.,   0.,   0.,  1.]])

 

###### DO NOT MODIFY ANYTHING HERE #######

 

filter(x, P)

 

matrix 类的代码跟上节中的相同。如下：

# -*- coding:utf-8 -*-

from math import*

 

 

class matrix:

   

    # implements basic operations of a matrixclass

   

    def __init__(self, value):

        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 sizeof matrix")

        else:

            self.dimx = dimx

            self.dimy = dimy

            self.value = [[0 for row inrange(dimy)] for col in range(dimx)]

   

    def identity(self, dim):

        # check if valid dimension

        if dim < 1:

            raise ValueError("Invalid sizeof matrix")

        else:

            self.dimx = dim

            self.dimy = dim

            self.value = [[0 for row inrange(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("Matricesmust 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("Matricesmust 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("Matricesmust be m*n and n*p to multiply")

        else:

            # multiply 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 ofCholesky and CholeskyInverse functions

   

    def Cholesky(self, ztol=1.0e-5):

        # Computes the upper triangularCholesky 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 kin range(i)])

            d = self.value[i][i] - S

            if abs(d) < ztol:

                res.value[i][i] = 0.0

            else:

                if d < 0.0:

                    raiseValueError("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

                try:

                   res.value[i][j] =(self.value[i][j] - S)/res.value[i][i]

                except:

                   raise ValueError("Zerodiagonal")

       return res

   

    def CholeskyInverse(self):

        # Computes inverse of matrix given itsCholesky 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)

 

 

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