Related
Attached is a simple python Kalman filter example of a free-fall object (g=-9.8m/s^2)
Alas, I have a problem. The state vector x contains both the position and the velocity but the z vector (measurement) contains only the position.
If I set a wrong initial position value, the algorithm coverages to the true value even with noisy measurements (see picture below)
However, if I sent the wrong initial velocity value, the algorithm does not converge even though the motion model is defined correctly.
Attached is the python code:
kalman.py
In your code I see two problems.
You set the Q-Matrix to zero. It means you trust too much in your model and give the filter no chance to improve the estimation through the measurement. Your filter becomes to stiff. You can think of it like a low pass filter with a very big time constant.
In my code I set the Q-Matrix to
Q = np.array([[1,0],[0,0.1]])
The second issue is your measurement noise. You simulate the noisy measurements with R=100 but communicate to the filter R=4. The filter trusts the measurement more than it should be. This issue is not really relevant to your question but still it should be corrected.
Now even if I set the initial velocity to 20, the position estimation works fine.
Here is the estimation for R = 4:
And for R = 100:
UPDATE
The velocity estimation works wrong, because you have some mistakes in your matrix operations. Please note, the matrix multiplication goes through np.dot(), not through *.
Here is a correct result for v0 = 20:
Many thanks, Anton.
Attached below is the corrected code for your convenience:
Roi
import numpy as np
import matplotlib.pyplot as plt
%matplotlib notebook
from numpy.linalg import inv
N = 1000 # number of time steps
dt = 0.01 # Sampling time (s)
t = dt*np.arange(N)
F = np.array([[1, dt],[ 0, 1]])# system matrix - state
B = np.array([[-1/2*dt**2],[ -dt]])# system matrix - input
H = np.array([[1, 0]])#; % observation matrix
Q = np.array([[1,0],[0,1]])
u = 9.80665# % input = acceleration due to gravity (m/s^2)
I = np.array([[1,0],[0,1]]) #identity matrix
# Define the initial position and velocity
y0 = 100; # m
v0 = 0; # m/s
G2 = np.array([-1/2*dt**2, -dt])# system matrix - input
# Initialize the state vector (true state)
xt = np.zeros((2, N)) # True state vector
xt[:,0] = [y0,v0]
for k in range(1,N):
xt[:,k] = np.dot(F,xt[:,k-1]) +G2*u
#Generate the noisy measurement from the true state
R = 4 # % m^2/s^2
v = np.sqrt(R)*np.random.randn(N) #% measurement noise
z = np.dot(H,xt) + v; #% noisy measurement
R2=4
#% Initialize the covariance matrix
P = np.array([[10, 0], [0, 0.1]])# Covariance for initial state error
#% Loop through and perform the Kalman filter equations recursively
x_list =[]
x_kalman= np.array([[117],[290]])
x_list.append(x_kalman)
print(-B*u)
for k in range(1,N):
x_kalman=np.dot(F,x_kalman) +B*u
P = np.dot(np.dot(F,P),F.T) +Q
S=(np.dot(np.dot(H,P),H.T) + R2)
S2 = inv(S)
K = np.dot(P,H.T)*S2
x_kalman = x_kalman +K*((z[:,k]- np.dot(H,x_kalman)))
P = np.dot((I - K*H),P)
x_list.append(x_kalman)
x_array = np.array(x_list)
print(x_array.shape)
plt.figure()
plt.plot(t,z[0,:], label="measurment", color='LIME', linewidth=1)
plt.plot(t,x_array[:,0,:],label="kalman",linewidth=5)
plt.plot(t,xt[0,:],linestyle='--', label = "Truth",linewidth=6)
plt.legend(fontsize=30)
plt.grid(True)
plt.xlabel("t[s]")
plt.title("Position Estimation", fontsize=20)
plt.ylabel("$X_t$ = h[m]")
plt.gca().set( ylim=(0, 110))
plt.gca().set(xlim=(0,6))
plt.figure()
#plt.plot(t,z, label="measurment", color='LIME')
plt.plot(t,x_array[:,1,:],label="kalman",linewidth=4)
plt.plot(t,xt[1,:],linestyle='--', label = "Truth",linewidth=2)
plt.legend()
plt.grid(True)
plt.xlabel("t[s]")
plt.title("Velocity Estimation")
plt.ylabel("$X_t$ = h[m]")
I need help with curve fitting a given set of points. The points form a parabola and I ought to find the peak point of the result. Issue is when I do a curve fit, it sometimes doesn't touch the max y-coordinate even if the actual point is given in the input array.
Following is the code snippet. Here 1.88 is the actual peak y-coordinate (13.05,1.88). But the graph generated by the code does not touch the point due to curve fitting. So is there a way to fit the curve making sure that it touches the max point given in the input array?
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import curve_fit, minimize_scalar
fig = plt.gcf()
#fig.set_size_inches(18.5, 10.5)
x = [4.59,9.02,13.05,18.47,20.3]
y = [1.7,1.84,1.88,1.7,1.64]
def f(x, p1, p2, p3):
return p3*(p1/((x-p2)**2 + (p1/2)**2))
plt.plot(x,y,"ro")
popt, pcov = curve_fit(f, x, y)
# find the peak
fm = lambda x: -f(x, *popt)
r = minimize_scalar(fm, bounds=(1, 5))
print( "maximum:", r["x"], f(r["x"], *popt) ) #maximum: 2.99846874275 18.3928199902
plt.text(1,1.9,'maximum '+str(round(r["x"],2))+'( #'+str(round(f(r["x"], *popt),2)) + ' )')
x_curve = np.linspace(min(x), max(x), 50)
plt.plot(x_curve, f(x_curve, *popt))
plt.plot(r['x'], f(r['x'], *popt), 'ko')
plt.show()
Here is a graphical code example using your equation with weighted fitting, where I have made the max point larger to more easily see the effect of the weighting. In non-weighted curve fitting, all weights are implicitly 1.0 as all data points have equal weight. Scipy's curve_fit routine uses weights in the form of uncertainties, so that giving a point a very small uncertainty (which I have done) is like giving the point a very large weight. This technique can be used to make a fit pass arbitrarily close to any single data point by any software that can perform weghted fitting.
import numpy, scipy, matplotlib
import matplotlib.pyplot as plt
from scipy.optimize import curve_fit
x = [4.59,9.02,13.05,18.47,20.3]
y = [1.7,1.84,2.0,1.7,1.64]
# note the single very small uncertainty - try making this value 1.0
uncertainties = numpy.array([1.0, 1.0, 1.0E-6, 1.0, 1.0])
# rename data to use previous example
xData = numpy.array(x)
yData = numpy.array(y)
def func(x, p1, p2, p3):
return p3*(p1/((x-p2)**2 + (p1/2)**2))
# these are the same as the scipy defaults
initialParameters = numpy.array([1.0, 1.0, 1.0])
# curve fit the test data, first without uncertainties to
# get us closer to initial starting parameters
ssqParameters, pcov = curve_fit(func, xData, yData, p0 = initialParameters)
# now that we have better starting parameters, use uncertainties
fittedParameters, pcov = curve_fit(func, xData, yData, p0 = ssqParameters, sigma=uncertainties, absolute_sigma=True)
modelPredictions = func(xData, *fittedParameters)
absError = modelPredictions - yData
SE = numpy.square(absError) # squared errors
MSE = numpy.mean(SE) # mean squared errors
RMSE = numpy.sqrt(MSE) # Root Mean Squared Error, RMSE
Rsquared = 1.0 - (numpy.var(absError) / numpy.var(yData))
print('Parameters:', fittedParameters)
print('RMSE:', RMSE)
print('R-squared:', Rsquared)
print()
##########################################################
# graphics output section
def ModelAndScatterPlot(graphWidth, graphHeight):
f = plt.figure(figsize=(graphWidth/100.0, graphHeight/100.0), dpi=100)
axes = f.add_subplot(111)
# first the raw data as a scatter plot
axes.plot(xData, yData, 'D')
# create data for the fitted equation plot
xModel = numpy.linspace(min(xData), max(xData))
yModel = func(xModel, *fittedParameters)
# now the model as a line plot
axes.plot(xModel, yModel)
axes.set_xlabel('X Data') # X axis data label
axes.set_ylabel('Y Data') # Y axis data label
plt.show()
plt.close('all') # clean up after using pyplot
graphWidth = 800
graphHeight = 600
ModelAndScatterPlot(graphWidth, graphHeight)
When using simplex noise one of it's main features is in the generation of gradients on-the-fly. This algorithm is described here. The problem is that even in the patent the gradient generation algorithm is only described in three dimensions (see below).
The specific new technique is as follows: The six bit index is split into (i) a lower three bit quantity, which is used to compute a magnitude of either zero or one for each of x,y and z, and (ii) an upper three bit quantity, which is used to determine an octant for the resulting gradient (positive or negative sign in each of x,y, and z).
If bit1bit0=0, then let (p,q,r)=(x,y,z). Otherwise, let (p,q,r) be a rotation of the order of (x,y,z) to (y,z,x) or (z,x,y), as bit1bit0=1 or 2, respectively, and set either q or r to zero as bit2=0 or 1, respectively.
000 p = x q = y r = z
001 p = y q = z r = 0
010 p = z q = x r = 0
011 p = x q = y r = 0
100 p = x q = y r = z
101 p = y q = 0 r = x
110 p = z q = 0 r = y
111 p = x q = 0 r = z
Then you basically continue and flip the signs of the components of the generated gradient.
The problem is basically how does the above rotation algorithm translate to 2D?
I implemented the thin plate spline algorithm (see also this description) in order to interpolate scattered data using Python.
My algorithm seems to work correctly when the bounding box of the initial scattered data has an aspect ratio close to 1. However, scaling one of the data points coordinates changes the interpolation result. I created a minimal working example that is representative of what I am trying to accomplish. Below are two plots showing the results of the interpolation of 50 random points.
First, the interpolation of z = x^2 on the domain x = [0, 3], y = [0, 120]:
As you can see, the interpolation fails. Now, executing the same process but after scaling the x values by a factor of 40, I get:
This time, the result looks better. Choosing a slightly different scaling factor would have resulted in a slightly different interpolation. This shows that something is wrong in my algorithm but I can't find what exactly. Here is the algorithm:
import numpy as np
import numba as nb
# pts1 = Mx2 matrix (original coordinates)
# z1 = Mx1 column vector (original values)
# pts2 = Nx2 matrix (interpolation coordinates)
def gen_K(n, pts1):
K = np.zeros((n,n))
for i in range(0,n):
for j in range(0,n):
if i != j:
r = ( (pts1[i,0] - pts1[j,0])**2.0 + (pts1[i,1] - pts1[j,1])**2.0 )**0.5
K[i,j] = r**2.0*np.log(r)
return K
def compute_z2(m, n, pts1, pts2, coeffs):
z2 = np.zeros((m,1))
x_min = np.min(pts1[:,0])
x_max = np.max(pts1[:,0])
y_min = np.min(pts1[:,1])
y_max = np.max(pts1[:,1])
for k in range(0,m):
pt = pts2[k,:]
# If point is located inside bounding box of pts1
if (pt[0] >= x_min and pt[0] <= x_max and pt[1] >= y_min and pt[1] <= y_max):
z2[k,0] = coeffs[-3,0] + coeffs[-2,0]*pts2[k,0] + coeffs[-1,0]*pts2[k,1]
for i in range(0,n):
r2 = ( (pts1[i,0] - pts2[k,0])**2.0 + (pts1[i,1] - pts2[k,1])**2.0 )**0.5
if r2 != 0:
z2[k,0] += coeffs[i,0]*( r2**2.0*np.log(r2) )
else:
z2[k,0] = np.nan
return z2
gen_K_nb = nb.jit(nb.float64[:,:](nb.int64, nb.float64[:,:]), nopython = True)(gen_K)
compute_z2_nb = nb.jit(nb.float64[:,:](nb.int64, nb.int64, nb.float64[:,:], nb.float64[:,:], nb.float64[:,:]), nopython = True)(compute_z2)
def TPS(pts1, z1, pts2, factor):
n, m = pts1.shape[0], pts2.shape[0]
P = np.hstack((np.ones((n,1)),pts1))
Y = np.vstack((z1, np.zeros((3,1))))
K = gen_K_nb(n, pts1)
K += factor*np.identity(n)
L = np.zeros((n+3,n+3))
L[0:n, 0:n] = K
L[0:n, n:n+3] = P
L[n:n+3, 0:n] = P.T
L_inv = np.linalg.inv(L)
coeffs = L_inv.dot(Y)
return compute_z2_nb(m, n, pts1, pts2, coeffs)
Finally, here is the code snippet I used to create the two plots:
import matplotlib.pyplot as plt
import numpy as np
N = 50 # Number of random points
pts = np.random.rand(N,2)
pts[:,0] *= 3.0 # initial x values
pts[:,1] *= 120.0 # initial y values
z1 = (pts[:,0])**2.0
for scale in [1.0, 40.0]:
pts1 = pts.copy()
pts1[:,0] *= scale
x2 = np.linspace(np.min(pts1[:,0]), np.max(pts1[:,0]), 40)
y2 = np.linspace(np.min(pts1[:,1]), np.max(pts1[:,1]), 40)
x2, y2 = np.meshgrid(x2, y2)
pts2 = np.vstack((x2.flatten(), y2.flatten())).T
z2 = TPS(pts1, z1.reshape(z1.shape[0], 1), pts2, 0.0)
# Display
fig = plt.figure(figsize=(4,3))
ax = fig.add_subplot(111)
C = ax.contourf(x2, y2, z2.reshape(x2.shape), np.linspace(0,9,10), extend='both')
ax.plot(pts1[:,0], pts1[:,1], 'ok')
ax.set_xlabel('x')
ax.set_ylabel('y')
plt.colorbar(C, extendfrac=0)
plt.tight_layout()
plt.show()
Thin Plate Spline is scalar invariant, which means if you scale x and y by the same factor, the result should be the same. However, if you scale x and y differently, then the result will be different. This is common characteristics among radial basis functions. Some radial basis functions are not even scalar invariant.
When you say it "fails", what do you mean? The big question is, does it still exactly interpolate at the construction points? Assuming your code is correct and you do not have ill-conditioning, it should in which case it does not fail.
What I think is happening is that the addition of the scale is making the behavior in the x direction more dominant so you do not see the wiggles that come naturally from the interpolation.
As an aside, you can greatly speed up your code without using Numba by vectorizing.
import scipy.spatial.distance
import scipy.special
def gen_K(n,pts1):
# No need for n but kept to maintain compatability
pts1 = np.atleast_2d(pts1)
r = scipy.spatial.distance.cdist(pts1,pts1)
return scipy.special.xlogy(r**2,r)
It means you will get horrible ridges running through the surface. Resulting in a sub-optimal model fit. Read the caption below the images. Your model is experiencing the same effect, although plotted in 2D.
Anyone please explain what is being done with the following code.
The code performs image fusion using Stationary Wavelet Transform.
%image decomposition using discrete stationary wavelet transform
[A1L1,H1L1,V1L1,D1L1] = swt2(im1,1,'sym2');
[A2L1,H2L1,V2L1,D2L1] = swt2(im2,1,'sym2');
[A1L2,H1L2,V1L2,D1L2] = swt2(A1L1,1,'sym2');
[A2L2,H2L2,V2L2,D2L2] = swt2(A2L1,1,'sym2');
% fusion at level2
AfL2 = 0.5*(A1L2+A2L2); **what are these equations ?**
D = (abs(H1L2)-abs(H2L2))>=0;
HfL2 = D.*H1L2 + (~D).*H2L2;
D = (abs(V1L2)-abs(V2L2))>=0;
VfL2 = D.*V1L2 + (~D).*V2L2;
D = (abs(D1L2)-abs(D2L2))>=0;
DfL2 = D.*D1L2 + (~D).*D2L2;
% fusion at level1
D = (abs(H1L1)-abs(H2L1))>=0;
HfL1 = D.*H1L1 + (~D).*H2L1;
D = (abs(V1L1)-abs(V2L1))>=0;
VfL1 = D.*V1L1 + (~D).*V2L1;
D = (abs(D1L1)-abs(D2L1))>=0;
DfL1 = D.*D1L1 + (~D).*D2L1;
% fused image
AfL1 = iswt2(AfL2,HfL2,VfL2,DfL2,'sym2');
imf = iswt2(AfL1,HfL1,VfL1,DfL1,'sym2');
Here AfL2, HfL2, VfL2, DfL2 at Fusion Level 2 are
Approximation coefficients
Horizontal detail coefficients
Vertical detail coefficients
Diagonal detail coefficients
Also same at next level and their respective mathematical implementations according to the concept.
It is really important to read the concept documents once so that you can understand the implementation easily, you can find the info's from the following link, you can directly move to the block diagram which explains the concept and then to physical implementation:
http://ijeetc.com/ijeetcadmin/upload/IJEETC_50e52508758cf.pdf