三次样条插值特点与实现 (引用了一点别人代码,但做了改动!)

本文介绍了一种基于三次样条插值的曲线拟合算法,该算法将一系列离散数据点平滑连接起来,形成一条连续且光滑的曲线。通过计算一阶和二阶导数,确保了曲线在每个数据点处的连续性和光滑性。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

用了点别人代码,但做了一些改动,因为看不懂那人的代码:

引用部分原文: http://haoxiang.org/2011/06/cubic-spline-interpolation-curve-fitting/

 

一. 主要特点

  1.)  一阶导数是二次曲线(光滑的..) 

     2.) 二阶导数是线性的

     3.) 也就是说,三次样条插值,是把N个离散数据,变成N-1个段,每一段的一次,二次都是连续的!

     4.) 计算量大点...

二. 实现


#define MAX_LEN_3 (100)

/
void CCubicSpline::derivative(const double *pXi,const double *pYi, int len, double *pCoefs, double Ml, double Mr)
{

 

double d[MAX_COUNT] = {0.0}; //右值

double h[MAX_COUNT] = {0.0};
double u[MAX_COUNT] = {0.0};
double lam[MAX_COUNT] = {0.0};

//一阶导数
for (int i = 0; i<len-1; i++)
{
h[i] = pXi[i+1] - pXi[i];
}

//计算矩阵值
for (int i = 1; i<len-1; i++)
{
u[i] = h[i-1]/(h[i] + h[i - 1]);
lam[i] = h[i] /(h[i] + h[i - 1]);
}


double m[MAX_COUNT*MAX_COUNT] = {0.0};

double a[MAX_COUNT] = {0.0};
double b[MAX_COUNT] = {0.0};
double c[MAX_COUNT] = {0.0};

//填冲..
for (int i = 0; i<len; i++)
{
for (int j = 0; j<len; j++)
{
m[i*len + j] = 0.0;
}

if (i == 0)
{
m[i*len + 0] = 2;
m[i*len + 1] = 1;

b[0] = 2;
c[0] = 1;
}
else if (i == (len - 1))
{
m[i*len + len - 2] = 1;
m[i*len + len - 1] = 2;

a[len -1] = 1;
b[len -1] = 2;
}
else
{
m[i*len + i-1] = u[i];
m[i*len + i ] = 2;
m[i*len + i+1] = lam[i];

a[i] = u[i];
b[i] = 2;
c[i] = lam[i];
}
}

 

// 计算方程右面
double g[MAX_COUNT] = {0.0};

// double Ml = 1.0; //左 指定的系数
// double Mr = 0.6868; //右 指定的系数
g[0] =( 6 * ( (pYi[1] - pYi[0])/ h[0] - Ml) )/h[0];
g[len - 1] = 6 * ( Mr - (pYi[len - 1] - pYi[len - 2])/h[len - 2] )/h[len - 2];

// g[0] =0.0;
// g[len - 1] = 0.0;


for (int i = 1; i < len - 1; i++)
{
double tmp1 = ((pYi[i+1] - pYi[i])/h[i]);
double tmp2 = ((pYi[i] - pYi[i-1])/ h[i-1]);
g[i] =( tmp1 - tmp2 ) / (h[i-1] + h[i]) * 6;
}


printf("\n====================\n");
for (int i=0; i<len; i++)
{
for (int j=0; j<len; j++)
{
printf(" %7.3f ", m[i*len + j]);
}
printf(" * M%d : %7.3f \n", i, g[i]);
}

int count = len - 2;

printf("\n=========\n");
for (int j=0; j<count; j++)
{
printf(" %7.3f %7.3f %7.3f * M%d : %7.3f \n", a[j], b[j], c[j], j, g[j]);
}

 

//b[1] = a[1]; c[1]
//
//
// 解方程 --> 追赶法
double p[MAX_COUNT] = {0.0};
p[0] = c[0]/b[0];
for (int i = 1; i<len - 1;i++)
{
p[i] = c[i]/(b[i] - a[i]*p[i-1]);
}


double y[MAX_COUNT] = {0.0};
y[0] = g[0]/b[0];
for (int i = 1; i<len;i++)
{
y[i] = (g[i] - a[i]*y[i-1])/(b[i] - a[i]*p[i-1]);
}

double M[MAX_COUNT] = {0.0};

M[len -1] = y[len -1];
for (int i=len-2; i>=0; i--)
{
M[i] = y[i] - p[i]*M[i+1];
}


printf("\n P:");
for (int i=0; i<len; i++)
{
printf(" %9.5f ", p[i]);
}

printf("\n Y:");
for (int i=0; i<len; i++)
{
printf(" %9.5f ", y[i]);
}

printf("\n M:");
for (int i=0; i<len; i++)
{
printf(" %9.5f ", M[i]);
}
printf("\n");


//代入三次样表的表达式

for (int i=0; i<len-1; i++)
{
pCoefs[i*4 + 0] = M[i+0] / (6 * h[i]);
pCoefs[i*4 + 1] = (pYi[i+0] - M[i+0] * h[i] * h[i] / 6) / h[i];
pCoefs[i*4 + 2] = M[i+1] / (6 * h[i]);
pCoefs[i*4 + 3] = (pYi[i+1] - M[i+1] * h[i] * h[i] / 6) / h[i];
}

}

 

//////////////////////////////

 

 


void CCubicSpline::derivative(const double *pXi,const double *pYi, int len, double *pCoefs)
{

int count = len - 2;


double d[MAX_COUNT] = {0.0}; //右值

double h[MAX_COUNT] = {0.0};
double u[MAX_COUNT] = {0.0};
double lam[MAX_COUNT] = {0.0};

//一阶导数
for (int i = 0; i<len-1; i++)
{
h[i] = pXi[i+1] - pXi[i];
}

//计算矩阵值
for (int i = 1; i<len-1; i++)
{
u[i] = h[i-1]/(h[i] + h[i - 1]);
lam[i] = h[i] /(h[i] + h[i - 1]);
}


double m[MAX_COUNT*MAX_COUNT] = {0.0};

double a[MAX_COUNT] = {0.0};
double b[MAX_COUNT] = {0.0};
double c[MAX_COUNT] = {0.0};

//填冲..
for (int i = 0; i<len; i++)
{
for (int j = 0; j<len; j++)
{
m[i*len + j] = 0.0;
}

if (i == 0)
{
m[i*len + 0] = 2;
m[i*len + 1] = 1;

}
else if (i == (len - 1))
{
m[i*len + len - 2] = 1;
m[i*len + len - 1] = 2;


}
else
{
m[i*len + i-1] = u[i];
m[i*len + i ] = 2;
m[i*len + i+1] = lam[i];

a[i-1] = u[i];
b[i-1] = 2;
c[i-1] = lam[i];
}
}

a[0] = 0.0;
c[count - 1] = 0.0;

// 计算方程右面
double g[MAX_COUNT] = {0.0};


for (int i = 1; i < len - 1; i++)
{
double tmp1 = ((pYi[i+1] - pYi[i])/h[i]);
double tmp2 = ((pYi[i] - pYi[i-1])/ h[i-1]);
g[i-1] =( tmp1 - tmp2 ) / (h[i-1] + h[i]) * 6;
}


printf("\n====================\n");
for (int i=0; i<len; i++)
{
for (int j=0; j<len; j++)
{
printf(" %7.3f ", m[i*len + j]);
}
printf(" * M%d : %7.3f \n", i, g[i]);
}


printf("\n=========\n");
for (int j=0; j<count; j++)
{
printf(" %7.3f %7.3f %7.3f * M%d : %7.3f \n", a[j], b[j], c[j], j, g[j]);
}

 

//b[1] = a[1]; c[1]
//
//
// 解方程 --> 追赶法
double p[MAX_COUNT] = {0.0};
p[0] = c[0]/b[0];
for (int i = 1; i<count - 1;i++)
{
p[i] = c[i]/(b[i] - a[i]*p[i-1]);
}


double y[MAX_COUNT] = {0.0};
y[0] = g[0]/b[0];
for (int i = 1; i<count;i++)
{
y[i] = (g[i] - a[i]*y[i-1])/(b[i] - a[i]*p[i-1]);
}

double M[MAX_COUNT] = {0.0};

M[count -1] = y[count -1];
for (int i=count-1; i>=0; i--)
{
M[i] = y[i] - p[i]*M[i+1];
}

for (int i=count - 1; i>= 0; i--)
{
M[i+1] = M[i];
}
M[0] = 0.0;


printf("\n P:");
for (int i=0; i<count; i++)
{
printf(" %9.5f ", p[i]);
}

printf("\n Y:");
for (int i=0; i<count; i++)
{
printf(" %9.5f ", y[i]);
}

printf("\n M:");
for (int i=0; i<len-1; i++)
{
printf(" %9.5f ", M[i]);
}
printf("\n");


//代入三次样条的表达式

// for(i = 0;i <= pLine->point_num - 2;i++)
// {
// pLine->a3[i] = M[i] / (6 * H[i]);
// pLine->a1[i] = (pLine->y[i] - M[i] * H[i] * H[i] / 6) / H[i];
// pLine->b3[i] = M[i+1] / (6 * H[i]);
// pLine->b1[i] = (pLine->y[i+1] - M[i+1] * H[i] * H[i] / 6) /H[i];
// }


for (int i=0; i<len-1; i++)
{
pCoefs[i*4 + 0] = M[i+0] / (6 * h[i]);
pCoefs[i*4 + 1] = (pYi[i+0] - M[i+0] * h[i] * h[i] / 6) / h[i];
pCoefs[i*4 + 2] = M[i+1] / (6 * h[i]);
pCoefs[i*4 + 3] = (pYi[i+1] - M[i+1] * h[i] * h[i] / 6) / h[i];
}

 

 

 

 

}

 

double CCubicSpline::Spline3(double startX, double endX, int index, double curX)
{
double Coefs0 = m_pCoefs[index * 4 + 0];
double Coefs1 = m_pCoefs[index * 4 + 1];
double Coefs2 = m_pCoefs[index * 4 + 2];
double Coefs3 = m_pCoefs[index * 4 + 3];

double outY0 = Coefs0 * (endX - curX) * (endX - curX) * (endX - curX);
double outY1 = Coefs1 * (endX - curX);
double outY2 = Coefs2 * (curX - startX) * (curX - startX) * (curX - startX);
double outY3 = Coefs3 * (curX - startX);

 

return (outY0 + outY1 + outY2 + outY3);
}

 

void CCubicSpline::PushPoint(CPoint point)
{
vPoint.push_back(point);

// 计算系数

double xi[MAX_COUNT] = { 0.0};
double yi[MAX_COUNT] = { 0.0};
int count = vPoint.size();

for (int i=0; i<count; i++)
{
CPoint point = vPoint[i];
xi[i] = point.x;
yi[i] = point.y;
}


if (count < 4)
{
return;
}
derivative(xi,yi, count, m_pCoefs);


}

 

if (vPoint.size() > 3)
{
int startX = vPoint[0].x;
int EndX = 0;
pDC->MoveTo(vPoint[0].x,vPoint[0].y);
for (int i=1; i<vPoint.size(); i++)
{
EndX = vPoint[i].x;

for (int curX=startX; curX<EndX; )
{
double curY = this->Spline3(startX, EndX, i -1 , curX);
//pDC->Ellipse(curX-1,curY-1,curX+1,curY+1);

pDC->LineTo(curX, curY);

if (startX < EndX) curX++;
if (startX > EndX) curX--;

}
startX = EndX;


}


}

for (int i=0; i<vPoint.size(); i++)
{
CPoint point = vPoint[i];

pDC->Ellipse(point.x-3,point.y-3,point.x+3,point.y+3);
}

 

 

 

// 差不多全在这儿了!  运行还不错...

 

 

 

 

 

 

 

 

 

 

 

 

转载于:https://www.cnblogs.com/signal/archive/2012/03/30/2425679.html

观察两个代码,主代码为机器人端,控代码为遥控器端能否实现以下功能? a我需要一个六足的机器蜘蛛,每个足上有三个关节,通过18个舵机控制,头部有两个舵机控制摄像头的立体旋转,整个机器蜘蛛由Esp 32pac9685配合控制,Esp 32直接控制4个舵机,Pic 9685控制16个舵机,通过WIFI遥控器通信,遥控器也使用Esp 32编程,可分为遥控器端代码和机械,蜘蛛端代码,给我可行的,可以实施的不需要后期调整的,首先想一下引脚方面的连接应该如何配置?1,把引脚方面如何配置的方法发给我,还有各个舵机在机械蜘蛛中控制的哪些关节?之后,一部分一部分的发给我代码,根据图中模块,添加摄像头,注意性价比,不需要图片质量太好,通过遥控器端USB连接到手机或者电脑显示画面 b里面有陀螺仪,陀螺仪的x轴朝向机械蜘蛛的前端,你可以添加一些动作或者特殊姿势,有左前右三面红外避障,主控制器,更着重于整体的控制和计算,辅助控制器每个机械腿的长度,在代码中标注,我会在后面更改 c使用标准库,注意机械蜘蛛的动作,分别有行走前后左右方向都可以,通过遥控器的左手柄控制,头部摄像头通过右手柄控制,还有机械蜘蛛的速度通过手柄上的电位器控制,还有机械蜘蛛的跳跃,通过遥控器的按钮控制通常是向前跳跃,按住遥控器上的按钮蓄力,双击释放,松开则停止蓄力并恢复,并在遥控器上还有两个旋钮控制跳跃的高度和远度,机械蜘蛛上加装图中所示的摄像头,通过WIFI传输到遥控器,遥控器端通过USB传输到电脑或者手机端进行显示,并且在机械支柱上还配备有温湿度传感器,将收到的数据实时传递到遥控器,遥控器上连接液晶屏,实时显示各种状态,以及温湿度数据,并且液晶屏屏标注一下我可以改动的数据,遥控器机器蜘蛛各有一个按钮按下之后进行云端通信,机器蜘蛛端通过sim 800l接入网络,遥控器端通过esp 32接入手机热点或者WIFI接入网络,并且直接机械蜘蛛建立联系,再次按下进行局域网双向通信,此时不依靠外界网络,一折为局域网模式,一者接入此局域网,实现两者的互相通信,引脚可以自行分配,然后告知我,注意和现实紧密联系,不要浮想联翩,有什么问题向我提问 d加一些仿生的动作,波浪步,三角形步,螃蟹步遥控器端按钮控制/潜行遥控器端按钮控制/抬头低头遥控器端按钮控制双击抬头,单击低头/机器蜘蛛本体距离地面的距离,电位器调节/机械蜘蛛所有动作的速度,电位器调节/攻击姿态,抬起前肢,转为四足运动,并注意重心分配/招手动作,遥控器按钮控制/跳跃,遥控器,按钮控制,点击进入跳跃模式并进行储能,双击进行跳跃,单击恢复退出储能,并不进行跳跃,遥控器上两个电位器,分别控制高度和远度,通过添加遥控器按钮控制特别注意它的稳定性,可以实现自己调节 e使用adoine进行编程,给我直接可以烧录的代码,不要简单的示例和实验代码就要直接可以集成我的要求,并且部署的代码 f遥控器主芯片为Esp 321遥控器摇杆:遥控器左摇杆控制机器蜘蛛前后左右移动,左摇杆按钮,按一下,本体随蜘蛛前后左右移动方向变化,再按一下,本体方向固定,仅由蜘蛛腿进行前后左右移动,右摇杆控制机器人头部舵机,为摄像头提供180度转向空间,右摇杆按键,按一下摄像头回正2跳跃按键:按一下时准备跳跃,方向为前,双击,开始跳跃,单击取消3模式按键:按一下切换切换三角步,再按两下切换波动步,按三下切换螃蟹步,并且在液晶屏上显示状态4攻击按键:按一下,切换攻击状态,再按一下取消,按两下,招手(招手三次后恢复)5蜷缩按键:按一下进入蜷缩状态,再按一下退出6炫技按键:按一下六条腿按规律抬起,并放下,律动,再按一下取消7电位器a:调整跳跃高度,并且在液晶屏上显示8电位器b:调整跳跃距离,并且在液晶屏上显示9电位器c:调整机械蜘蛛本体距离地面高度,并在液晶屏上显示10电位器d:调整机器蜘蛛所有动作的执行速度11指纹传感器:解锁遥控状态12USB接口:传输到电脑或手机显示机械蜘蛛发送的视频图像 g使用配件 摇杆模块五针双轴按键pS 2 网络通讯Sim 800l(连接机器蜘蛛实现超远距离控制,遥控器通过调整esp 8266接入因特网进行控制) 温湿度传感器,Dht 11 舵机SG 90 陀螺仪,MpU 6050 摄像头,Ov 7670 红外避障模块 Pac9685 h注意我的要求,以及所有动作的时间都要完成 观察一下这两个代码,主代码为机器人端,控代码为遥控器端,仔细分析阅读,看他对我要求的完成度如何?
03-31
%% OFDM系统参数设置 clear all; close all; clc; % 基本参数 N = 64; % 子载波数量 cp_len = 16; % 循环前缀长度 num_symbols = 100; % OFDM符号数量 mod_order = 4; % 调制阶数 (4=QPSK) snr = 5:2:15; % 信噪比范围(dB) num_snr_points = length(snr); %% LDPC编码设置 % 检查并加载DVB-S2 LDPC矩阵 if ~exist('dvbs2ldpc.mat', 'file') fprintf('正在下载DVB-S2 LDPC矩阵...\n'); try websave('dvbs2ldpc.mat', 'https://www.mathworks.com/matlabcentral/mlc-downloads/downloads/submissions/46480/versions/1/dvbs2ldpc.mat'); fprintf('下载成功!\n'); catch error('无法下载dvbs2ldpc.mat文件,请检查网络连接'); end end load('dvbs2ldpc.mat'); % 选择码率 code_rate = 1/2; H = dvbs2ldpc(code_rate); ldpcEncoder = comm.LDPCEncoder(H); ldpcDecoder = comm.LDPCDecoder(H); % 获取LDPC编码参数 info_bits_per_block = ldpcEncoder.NumInformationBits; codeword_bits_per_block = ldpcEncoder.BlockLength; %% 数据长度计算对齐 % 计算每个OFDM符号能承载的比特数 bits_per_symbol = N * log2(mod_order); % 计算总需要的编码比特数 required_encoded_bits = bits_per_symbol * num_symbols; % 计算需要的LDPC块数 num_ldpc_blocks = ceil(required_encoded_bits / codeword_bits_per_block); total_info_bits = num_ldpc_blocks * info_bits_per_block; % 接收端实际能解码的LDPC块数 num_ldpc_codewords = floor(required_encoded_bits / codeword_bits_per_block); info_bits_actual = num_ldpc_codewords * info_bits_per_block; fprintf('系统参数:\n'); fprintf(' OFDM符号数: %d, 每符号子载波: %d, 总传输比特: %d\n', ... num_symbols, N, required_encoded_bits); fprintf(' LDPC码率: %.2f, 每块信息比特: %d, 码字长度: %d\n', ... code_rate, info_bits_per_block, codeword_bits_per_block); fprintf(' 发送端LDPC块数: %d, 接收端可解码块数: %d\n', ... num_ldpc_blocks, num_ldpc_codewords); fprintf(' 总信息比特: %d, 实际解码比特: %d\n\n', ... total_info_bits, info_bits_actual); %% BER存储 ber_results = zeros(num_snr_points, 1); last_constellation = []; % 存储最后一个SNR点的星座图 %% 主循环 - 不同SNR下的仿真 for snr_idx = 1:num_snr_points current_snr = snr(snr_idx); fprintf('\n正在仿真 SNR = %d dB...\n', current_snr); % 错误统计初始化 total_errors = 0; total_bits = 0; % 多次迭代以获得更准确的BER估计 num_iterations = 10; for iter = 1:num_iterations %% 发射端处理 % 生成随机二进制数据 (额外生成8%的冗余数据) data = randi([0 1], total_info_bits, 1); % LDPC编码 encoded_data = zeros(codeword_bits_per_block * num_ldpc_blocks, 1); for blk = 1:num_ldpc_blocks start_idx = (blk-1)*info_bits_per_block + 1; end_idx = blk*info_bits_per_block; encoded_block = ldpcEncoder(data(start_idx:end_idx)); encoded_data((blk-1)*codeword_bits_per_block+1 : blk*codeword_bits_per_block) = ... encoded_block; end % 截取刚好够用的编码后数据 tx_encoded = encoded_data(1:required_encoded_bits); % 数字调制 (QPSK) mod_data = qammod(tx_encoded, mod_order, 'InputType', 'bit', 'UnitAveragePower', true); % 串并转换 mod_data = reshape(mod_data, N, num_symbols); % IFFT变换 ofdm_symbols = ifft(mod_data, N); % 添加循环前缀 ofdm_symbols_cp = [ofdm_symbols(end-cp_len+1:end, :); ofdm_symbols]; % 并串转换 tx_signal = ofdm_symbols_cp(:); %% 信道传输 rx_signal = awgn(tx_signal, current_snr, 'measured'); %% 接收端处理 % 串并转换 rx_signal_parallel = reshape(rx_signal, N+cp_len, num_symbols); % 去除循环前缀 rx_symbols_no_cp = rx_signal_parallel(cp_len+1:end, :); % FFT变换 rx_data_freq = fft(rx_symbols_no_cp, N); % 信道均衡 (简单零 forcing均衡) H_est = ones(N, 1); % 假设理想信道估计 rx_data_eq = rx_data_freq ./ H_est; % 解调 - 输出LLR用于LDPC解码 rx_llr = qamdemod(rx_data_eq(:), mod_order, 'OutputType', 'approxllr', 'UnitAveragePower', true); %% LDPC解码 % 将接收数据分割为LDPC码字 rx_data_decoded = zeros(info_bits_actual, 1); for blk = 1:num_ldpc_codewords start_idx = (blk-1)*codeword_bits_per_block + 1; end_idx = min(blk*codeword_bits_per_block, length(rx_llr)); codeword_llr = rx_llr(start_idx:end_idx); % 解码时处理可能的LLR长度不足 if length(codeword_llr) < codeword_bits_per_block codeword_llr = [codeword_llr; zeros(codeword_bits_per_block-length(codeword_llr), 1)]; end decoded_bits = ldpcDecoder(codeword_llr); rx_data_decoded((blk-1)*info_bits_per_block+1 : blk*info_bits_per_block) = ... decoded_bits; end %% 性能评估 - 比较实际传输的信息比特 [errors, bits] = biterr(data(1:info_bits_actual), rx_data_decoded); total_errors = total_errors + errors; total_bits = total_bits + bits; % 保存最后一个SNR点的星座图 if snr_idx == num_snr_p修改为正确完整的代码
06-21
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值