【C++】代码实现:数据线性平滑算法:3点线性平滑、5点(1次、2次、3次)线性平滑、7点(1次、2次)线性平滑

  • 3点线性平滑:
//3点线性平滑
int LinearSmooth3(double* input, long size) {
	double *output = new double[size];
	long i(0);

	if (size < 3)
	{
		for (i = 0; i <= size - 1; i++)
		{
			output[i] = input[i];
		}
	}
	else
	{
		output[0] = (5.0 * input[0] + 2.0 * input[1] - input[2]) / 6.0;
		for (i = 1; i <= size - 2; i++)
		{
			output[i] = (input[i - 1] + input[i] + input[i + 1]) / 3.0;
		}
		output[size - 1] = (5.0 * input[size - 1] + 2.0 * input[size - 2] - input[size - 3]) / 6.0;
	}

	for (i = 0; i < size; i++)
	{
		input[i] = output[i];
	}

	delete[] output;

	return 0;
}
  • 5点线性平滑:
//5点线性平滑
int LinearSmooth5(double* input, long size) {
	double *output = new double[size];
	long i(0);

	if (size < 5)
	{
		for (i = 0; i <= size - 1; i++)
		{
			output[i] = input[i];
		}
	}
	else
	{
		output[0] = (3.0 * input[0] + 2.0 * input[1] + input[2] - input[4]) / 5.0;
		output[1] = (4.0 * input[0] + 3.0 * input[1] + 2 * input[2] + input[3]) / 10.0;
		for (i = 2; i <= size - 3; i++)
		{
			output[i] = (input[i - 2] + input[i - 1] + input[i] + input[i + 1] + input[i + 2]) / 5.0;
		}
		output[size - 2] = (4.0 * input[size - 1] + 3.0 * input[size - 2] + 2 * input[size - 3] + input[size - 4]) / 10.0;
		output[size - 1] = (3.0 * input[size - 1] + 2.0 * input[size - 2] + input[size - 3] - input[size - 5]) / 5.0;
	}

	for (i = 0; i < size; i++)
	{
		input[i] = output[i];
	}

	delete[] output;

	return 0;
}
  • 5点2次线性平滑:
//5点二次线性平滑
int LinearSmooth52(double* input, long size) {
	double *output = new double[size];
	long i(0);

	if (size < 5)
	{
		for (i = 0; i <= size - 1; i++)
		{
			output[i] = input[i];
		}
	}
	else
	{
		output[0] = (31.0 * input[0] + 9.0 * input[1] - 3.0 * input[2] - 5.0 * input[3] + 3.0 * input[4]) / 35.0;
		output[1] = (9.0 * input[0] + 13.0 * input[1] + 12 * input[2] + 6.0 * input[3] - 5.0 *input[4]) / 35.0;
		for (i = 2; i <= size - 3; i++)
		{
			output[i] = (-3.0 * (input[i - 2] + input[i + 2]) +
				12.0 * (input[i - 1] + input[i + 1]) + 17 * input[i]) / 35.0;
		}
		output[size - 2] = (9.0 * input[size - 1] + 13.0 * input[size - 2] + 12.0 * input[size - 3] + 6.0 * input[size - 4] - 5.0 * input[size - 5]) / 35.0;
		output[size - 1] = (31.0 * input[size - 1] + 9.0 * input[size - 2] - 3.0 * input[size - 3] - 5.0 * input[size - 4] + 3.0 * input[size - 5]) / 35.0;
	}

	for (i = 0; i < size; i++)
	{
		input[i] = output[i];
	}

	delete[] output;

	return 0;
}
  • 5点3次线性平滑:
//5点三次线性平滑
int LinearSmooth53(double* input, long size) {
	double *output = new double[size];
	long i(0);

	if (size < 5)
	{
		for (i = 0; i <= size - 1; i++)
			output[i] = input[i];
	}
	else
	{
		output[0] = (69.0 * input[0] + 4.0 * input[1] - 6.0 * input[2] + 4.0 * input[3] - input[4]) / 70.0;
		output[1] = (2.0 * input[0] + 27.0 * input[1] + 12.0 * input[2] - 8.0 * input[3] + 2.0 * input[4]) / 35.0;
		for (i = 2; i <= size - 3; i++)
		{
			output[i] = (-3.0 * (input[i - 2] + input[i + 2]) + 12.0 * (input[i - 1] + input[i + 1]) + 17.0 * input[i]) / 35.0;
		}
		output[size - 2] = (2.0 * input[size - 5] - 8.0 * input[size - 4] + 12.0 * input[size - 3] + 27.0 * input[size - 2] + 2.0 * input[size - 1]) / 35.0;
		output[size - 1] = (-input[size - 5] + 4.0 * input[size - 4] - 6.0 * input[size - 3] + 4.0 * input[size - 2] + 69.0 * input[size - 1]) / 70.0;
	}

	for (i = 0; i < size; i++)
	{
		input[i] = output[i];
	}

	delete[] output;

	return 0;
}
  • 7点线性平滑:
//7点线性平滑
int LinearSmooth7(double* input, long size) {
	double *output = new double[size];
	long i(0);

	if (size < 7)
	{
		for (i = 0; i <= size - 1; i++)
		{
			output[i] = input[i];
		}
	}
	else
	{
		output[0] = (13.0 * input[0] + 10.0 * input[1] + 7.0 * input[2] + 4.0 * input[3] +
			input[4] - 2.0 * input[5] - 5.0 * input[6]) / 28.0;
		output[1] = (5.0 * input[0] + 4.0 * input[1] + 3 * input[2] + 2 * input[3] +
			input[4] - input[6]) / 14.0;
		output[2] = (7.0 * input[0] + 6.0 * input[1] + 5.0 * input[2] + 4.0 * input[3] +
			3.0 * input[4] + 2.0 * input[5] + input[6]) / 28.0;
		for (i = 3; i <= size - 4; i++)
		{
			output[i] = (input[i - 3] + input[i - 2] + input[i - 1] + input[i] + input[i + 1] + input[i + 2] + input[i + 3]) / 7.0;
		}
		output[size - 3] = (7.0 * input[size - 1] + 6.0 * input[size - 2] + 5.0 * input[size - 3] +
			4.0 * input[size - 4] + 3.0 * input[size - 5] + 2.0 * input[size - 6] + input[size - 7]) / 28.0;
		output[size - 2] = (5.0 * input[size - 1] + 4.0 * input[size - 2] + 3.0 * input[size - 3] +
			2.0 * input[size - 4] + input[size - 5] - input[size - 7]) / 14.0;
		output[size - 1] = (13.0 * input[size - 1] + 10.0 * input[size - 2] + 7.0 * input[size - 3] +
			4 * input[size - 4] + input[size - 5] - 2 * input[size - 6] - 5 * input[size - 7]) / 28.0;
	}

	for (i = 0; i < size; i++)
	{
		input[i] = output[i];
	}

	delete[] output;

	return 0;
}
  • 7点2次线性平滑:
//7点二次线性平滑
int LinearSmooth72(double* input, long size) {
	double *output = new double[size];
	long i(0);

	if (size < 7)
	{
		for (i = 0; i <= size - 1; i++)
		{
			output[i] = input[i];
		}
	}
	else
	{
		output[0] = (32.0 * input[0] + 15.0 * input[1] + 3.0 * input[2] - 4.0 * input[3] -
			6.0 * input[4] - 3.0 * input[5] + 5.0 * input[6]) / 42.0;
		output[1] = (5.0 * input[0] + 4.0 * input[1] + 3.0 * input[2] + 2.0 * input[3] +
			input[4] - input[6]) / 14.0;
		output[2] = (1.0 * input[0] + 3.0 * input[1] + 4.0 * input[2] + 4.0 * input[3] +
			3.0 * input[4] + 1.0 * input[5] - 2.0 * input[6]) / 14.0;
		for (i = 3; i <= size - 4; i++)
		{
			output[i] = (-2.0 * (input[i - 3] + input[i + 3]) +
				3.0 * (input[i - 2] + input[i + 2]) +
				6.0 * (input[i - 1] + input[i + 1]) + 7.0 * input[i]) / 21.0;
		}
		output[size - 3] = (1.0 * input[size - 1] + 3.0 * input[size - 2] + 4.0 * input[size - 3] +
			4.0 * input[size - 4] + 3.0 * input[size - 5] + 1.0 * input[size - 6] - 2.0 * input[size - 7]) / 14.0;
		output[size - 2] = (5.0 * input[size - 1] + 4.0 * input[size - 2] + 3.0 * input[size - 3] +
			2.0 * input[size - 4] + input[size - 5] - input[size - 7]) / 14.0;
		output[size - 1] = (32.0 * input[size - 1] + 15.0 * input[size - 2] + 3.0 * input[size - 3] -
			4.0 * input[size - 4] - 6.0 * input[size - 5] - 3.0 * input[size - 6] + 5.0 * input[size - 7]) / 42.0;
	}

	for (i = 0; i < size; i++)
	{
		input[i] = output[i];
	}

	delete[] output;

	return 0;
}

 

  • 10
    点赞
  • 86
    收藏
    觉得还不错? 一键收藏
  • 5
    评论
斯坦利跟踪算法是一种常用的自动驾驶中的路径跟踪算法,它可以根据车辆当前位置和车道线信息来生成转向控制指令。以下是一个简单的C++实现示例: ```cpp #include <iostream> #include <vector> #include <cmath> // 定义多项式拟合函数 std::vector<double> polyfit(const std::vector<double>& x, const std::vector<double>& y, int degree) { int n = x.size(); int m = degree + 1; std::vector<double> X(n * m); std::vector<double> Y(n); for (int i = 0; i < n; i++) { double xi = x[i]; double yi = y[i]; for (int j = 0; j < m; j++) { X[i * m + j] = std::pow(xi, j); } Y[i] = yi; } std::vector<double> coeff(m); // 使用适当的方法求解多项式系数,这里简单地用最小二乘法求解 // ... return coeff; } // 模拟获取车道线坐标的函数,这里假设返回一组随机坐标 std::vector<double> getLaneCoordinates() { std::vector<double> x = {0, 1, 2, 3, 4, 5}; std::vector<double> y = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6}; return y; } // 模拟获取车辆当前位置的函数,这里假设返回一个随机值 double getCurrentPosition() { return 2.5; } // 模拟斯坦利跟踪算法 void stanleyTracking() { while (true) { // 获取车道线坐标 std::vector<double> y = getLaneCoordinates(); // 进行多项式拟合,这里使用三拟合 std::vector<double> coeff = polyfit(y, y, 3); // 获取车辆当前位置 double currentPosition = getCurrentPosition(); // 计算拟合曲线上与车辆当前位置最近的的坐标 double nearestPoint = 0.0; double minDistance = std::numeric_limits<double>::max(); for (double i = 0.0; i <= 5.0; i += 0.1) { double distance = std::abs(coeff[3] * std::pow(i, 3) + coeff[2] * std::pow(i, 2) + coeff[1] * i + coeff[0] - currentPosition); if (distance < minDistance) { minDistance = distance; nearestPoint = i; } } // 计算横向偏移量 double offset = coeff[3] * std::pow(nearestPoint, 3) + coeff[2] * std::pow(nearestPoint, 2) + coeff[1] * nearestPoint + coeff[0] - currentPosition; // 计算转向控制指令 double headingError = std::atan(coeff[1] + 2 * coeff[2] * nearestPoint + 3 * coeff[3] * std::pow(nearestPoint, 2)); double crossTrackError = offset; double steerAngle = headingError + std::atan(1.0 / (crossTrackError + 0.0001)); // 避免除0错误 // 打印转向控制指令 std::cout << "Steer angle: " << steerAngle << std::endl; } } int main() { stanleyTracking(); return 0; } ``` 在实际应用中,你需要根据实际情况选择适当的多项式拟合方法,并进行参数调整和优化。此示例仅提供了一个简单的框架,你可以根据具体的需求进行进一步的开发和改进。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 5
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值