{"id":958994,"date":"2024-12-27T03:36:52","date_gmt":"2024-12-26T19:36:52","guid":{"rendered":"https:\/\/docs.pingcode.com\/ask\/ask-ask\/958994.html"},"modified":"2024-12-27T03:36:54","modified_gmt":"2024-12-26T19:36:54","slug":"ukf%e5%a6%82%e4%bd%95%e7%94%a8python%e5%ae%9e%e7%8e%b0","status":"publish","type":"post","link":"https:\/\/docs.pingcode.com\/ask\/958994.html","title":{"rendered":"ukf\u5982\u4f55\u7528python\u5b9e\u73b0"},"content":{"rendered":"<p style=\"text-align:center;\" ><img decoding=\"async\" src=\"https:\/\/cdn-kb.worktile.com\/kb\/wp-content\/uploads\/2024\/04\/25101928\/a3eeafaf-6373-47f4-aaeb-a9b759eb9a8e.webp\" alt=\"ukf\u5982\u4f55\u7528python\u5b9e\u73b0\" \/><\/p>\n<p><p> <strong>\u5728Python\u4e2d\u5b9e\u73b0\u65e0\u8ff9\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\uff08UKF\uff09\u7684\u6838\u5fc3\u5728\u4e8e\u7406\u89e3UKF\u7684\u57fa\u672c\u6982\u5ff5\u3001\u4f7f\u7528\u5408\u9002\u7684\u5e93\u4ee5\u53ca\u5904\u7406\u975e\u7ebf\u6027\u7cfb\u7edf\u3002\u4f7f\u7528Python\u5b9e\u73b0UKF\u7684\u6b65\u9aa4\u5305\u62ec\uff1a\u5b9a\u4e49\u72b6\u6001\u7a7a\u95f4\u6a21\u578b\u3001\u751f\u6210sigma\u70b9\u3001\u9884\u6d4b\u72b6\u6001\u548c\u534f\u65b9\u5dee\u3001\u66f4\u65b0\u6b65\u9aa4\u3002\u4e0b\u9762\u5c06\u8be6\u7ec6\u63cf\u8ff0\u5982\u4f55\u5728Python\u4e2d\u5b9e\u73b0UKF\u3002<\/strong><\/p>\n<\/p>\n<p><p><strong>\u5b9a\u4e49\u72b6\u6001\u7a7a\u95f4\u6a21\u578b<\/strong><br \/>\u65e0\u8ff9\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u9002\u7528\u4e8e\u975e\u7ebf\u6027\u7cfb\u7edf\uff0c\u9996\u5148\u9700\u8981\u5b9a\u4e49\u72b6\u6001\u8f6c\u79fb\u51fd\u6570\u548c\u89c2\u6d4b\u51fd\u6570\u3002\u72b6\u6001\u8f6c\u79fb\u51fd\u6570\u63cf\u8ff0\u7cfb\u7edf\u4ece\u4e00\u4e2a\u72b6\u6001\u5230\u4e0b\u4e00\u4e2a\u72b6\u6001\u7684\u8f6c\u6362\uff0c\u89c2\u6d4b\u51fd\u6570\u63cf\u8ff0\u5982\u4f55\u4ece\u72b6\u6001\u4e2d\u63d0\u53d6\u89c2\u6d4b\u503c\u3002\u53ef\u4ee5\u7528Python\u4e2d\u7684\u51fd\u6570\u6765\u5b9a\u4e49\u8fd9\u4e9b\u6a21\u578b\u3002<\/p>\n<\/p>\n<p><pre><code class=\"language-python\">import numpy as np<\/p>\n<p>def state_transition_function(state, control_input):<\/p>\n<p>    # \u5b9a\u4e49\u72b6\u6001\u8f6c\u79fb\u51fd\u6570<\/p>\n<p>    # \u793a\u4f8b\uff1a\u7b80\u5355\u7684\u7ebf\u6027\u8fd0\u52a8\u6a21\u578b<\/p>\n<p>    return np.dot(A, state) + np.dot(B, control_input)<\/p>\n<p>def observation_function(state):<\/p>\n<p>    # \u5b9a\u4e49\u89c2\u6d4b\u51fd\u6570<\/p>\n<p>    # \u793a\u4f8b\uff1a\u76f4\u63a5\u89c2\u6d4b\u72b6\u6001<\/p>\n<p>    return np.dot(H, state)<\/p>\n<p><\/code><\/pre>\n<\/p>\n<p><p><strong>\u751f\u6210Sigma\u70b9<\/strong><br \/>UKF\u7684\u6838\u5fc3\u5728\u4e8e\u4ece\u5f53\u524d\u4f30\u8ba1\u548c\u534f\u65b9\u5dee\u751f\u6210\u4e00\u7ec4sigma\u70b9\u3002\u8fd9\u4e9b\u70b9\u7528\u4e8e\u6355\u6349\u975e\u7ebf\u6027\u51fd\u6570\u7684\u7279\u5f81\u3002<\/p>\n<\/p>\n<p><pre><code class=\"language-python\">def generate_sigma_points(mean, covariance, alpha, beta, kappa):<\/p>\n<p>    n = mean.shape[0]<\/p>\n<p>    lambda_ = alpha2 * (n + kappa) - n<\/p>\n<p>    sigma_points = np.zeros((2 * n + 1, n))<\/p>\n<p>    weights_mean = np.zeros(2 * n + 1)<\/p>\n<p>    weights_covariance = np.zeros(2 * n + 1)<\/p>\n<p>    sigma_points[0] = mean<\/p>\n<p>    weights_mean[0] = lambda_ \/ (n + lambda_)<\/p>\n<p>    weights_covariance[0] = weights_mean[0] + (1 - alpha2 + beta)<\/p>\n<p>    sqrt_matrix = np.linalg.cholesky((n + lambda_) * covariance)<\/p>\n<p>    for i in range(n):<\/p>\n<p>        sigma_points[i + 1] = mean + sqrt_matrix[i]<\/p>\n<p>        sigma_points[n + i + 1] = mean - sqrt_matrix[i]<\/p>\n<p>        weights_mean[i + 1] = weights_covariance[i + 1] = 1 \/ (2 * (n + lambda_))<\/p>\n<p>        weights_mean[n + i + 1] = weights_covariance[n + i + 1] = 1 \/ (2 * (n + lambda_))<\/p>\n<p>    return sigma_points, weights_mean, weights_covariance<\/p>\n<p><\/code><\/pre>\n<\/p>\n<p><p><strong>\u9884\u6d4b\u6b65\u9aa4<\/strong><br \/>\u9884\u6d4b\u6b65\u9aa4\u5305\u62ec\u901a\u8fc7\u72b6\u6001\u8f6c\u79fb\u51fd\u6570\u9884\u6d4bsigma\u70b9\u3001\u8ba1\u7b97\u9884\u6d4b\u72b6\u6001\u548c\u9884\u6d4b\u534f\u65b9\u5dee\u3002<\/p>\n<\/p>\n<p><pre><code class=\"language-python\">def predict(sigma_points, weights_mean, weights_covariance, process_noise_covariance):<\/p>\n<p>    n_sigma = sigma_points.shape[0]<\/p>\n<p>    predicted_sigma_points = np.zeros(sigma_points.shape)<\/p>\n<p>    # \u901a\u8fc7\u72b6\u6001\u8f6c\u79fb\u51fd\u6570\u9884\u6d4bsigma\u70b9<\/p>\n<p>    for i in range(n_sigma):<\/p>\n<p>        predicted_sigma_points[i] = state_transition_function(sigma_points[i])<\/p>\n<p>    # \u8ba1\u7b97\u9884\u6d4b\u72b6\u6001<\/p>\n<p>    predicted_state = np.dot(weights_mean, predicted_sigma_points)<\/p>\n<p>    # \u8ba1\u7b97\u9884\u6d4b\u534f\u65b9\u5dee<\/p>\n<p>    predicted_covariance = np.zeros((predicted_state.shape[0], predicted_state.shape[0]))<\/p>\n<p>    for i in range(n_sigma):<\/p>\n<p>        diff = predicted_sigma_points[i] - predicted_state<\/p>\n<p>        predicted_covariance += weights_covariance[i] * np.outer(diff, diff)<\/p>\n<p>    predicted_covariance += process_noise_covariance<\/p>\n<p>    return predicted_state, predicted_covariance, predicted_sigma_points<\/p>\n<p><\/code><\/pre>\n<\/p>\n<p><p><strong>\u66f4\u65b0\u6b65\u9aa4<\/strong><br \/>\u66f4\u65b0\u6b65\u9aa4\u5305\u62ec\u901a\u8fc7\u89c2\u6d4b\u51fd\u6570\u9884\u6d4b\u89c2\u6d4b\u503c\u3001\u8ba1\u7b97Kalman\u589e\u76ca\u3001\u66f4\u65b0\u72b6\u6001\u548c\u534f\u65b9\u5dee\u3002<\/p>\n<\/p>\n<p><pre><code class=\"language-python\">def update(predicted_state, predicted_covariance, predicted_sigma_points, measurement, measurement_noise_covariance):<\/p>\n<p>    n_sigma = predicted_sigma_points.shape[0]<\/p>\n<p>    # \u901a\u8fc7\u89c2\u6d4b\u51fd\u6570\u9884\u6d4b\u89c2\u6d4b\u503c<\/p>\n<p>    predicted_measurements = np.zeros((n_sigma, measurement.shape[0]))<\/p>\n<p>    for i in range(n_sigma):<\/p>\n<p>        predicted_measurements[i] = observation_function(predicted_sigma_points[i])<\/p>\n<p>    # \u8ba1\u7b97\u9884\u6d4b\u89c2\u6d4b\u503c<\/p>\n<p>    predicted_measurement = np.dot(weights_mean, predicted_measurements)<\/p>\n<p>    # \u8ba1\u7b97\u89c2\u6d4b\u534f\u65b9\u5dee<\/p>\n<p>    measurement_covariance = np.zeros((measurement.shape[0], measurement.shape[0]))<\/p>\n<p>    for i in range(n_sigma):<\/p>\n<p>        diff = predicted_measurements[i] - predicted_measurement<\/p>\n<p>        measurement_covariance += weights_covariance[i] * np.outer(diff, diff)<\/p>\n<p>    measurement_covariance += measurement_noise_covariance<\/p>\n<p>    # \u8ba1\u7b97\u72b6\u6001\u548c\u89c2\u6d4b\u4e4b\u95f4\u7684\u534f\u65b9\u5dee<\/p>\n<p>    state_measurement_covariance = np.zeros((predicted_state.shape[0], measurement.shape[0]))<\/p>\n<p>    for i in range(n_sigma):<\/p>\n<p>        diff_state = predicted_sigma_points[i] - predicted_state<\/p>\n<p>        diff_measurement = predicted_measurements[i] - predicted_measurement<\/p>\n<p>        state_measurement_covariance += weights_covariance[i] * np.outer(diff_state, diff_measurement)<\/p>\n<p>    # \u8ba1\u7b97Kalman\u589e\u76ca<\/p>\n<p>    kalman_g<a href=\"https:\/\/docs.pingcode.com\/blog\/59162.html\" target=\"_blank\">AI<\/a>n = np.dot(state_measurement_covariance, np.linalg.inv(measurement_covariance))<\/p>\n<p>    # \u66f4\u65b0\u72b6\u6001\u548c\u534f\u65b9\u5dee<\/p>\n<p>    updated_state = predicted_state + np.dot(kalman_gain, (measurement - predicted_measurement))<\/p>\n<p>    updated_covariance = predicted_covariance - np.dot(kalman_gain, np.dot(measurement_covariance, kalman_gain.T))<\/p>\n<p>    return updated_state, updated_covariance<\/p>\n<p><\/code><\/pre>\n<\/p>\n<p><p><strong>\u521d\u59cb\u5316UKF\u53c2\u6570<\/strong><br \/>\u5728\u5b9e\u73b0\u65e0\u8ff9\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u65f6\uff0c\u9700\u8981\u521d\u59cb\u5316\u76f8\u5173\u53c2\u6570\uff0c\u5982\u521d\u59cb\u72b6\u6001\u4f30\u8ba1\u3001\u534f\u65b9\u5dee\u77e9\u9635\u3001\u8fc7\u7a0b\u566a\u58f0\u548c\u6d4b\u91cf\u566a\u58f0\u7b49\u3002<\/p>\n<\/p>\n<p><pre><code class=\"language-python\"># \u521d\u59cb\u72b6\u6001\u4f30\u8ba1\u548c\u534f\u65b9\u5dee<\/p>\n<p>initial_state_estimate = np.array([0, 0])<\/p>\n<p>initial_covariance = np.eye(2)<\/p>\n<h2><strong>\u8fc7\u7a0b\u566a\u58f0\u548c\u6d4b\u91cf\u566a\u58f0\u534f\u65b9\u5dee<\/strong><\/h2>\n<p>process_noise_covariance = np.eye(2) * 0.1<\/p>\n<p>measurement_noise_covariance = np.eye(2) * 0.1<\/p>\n<h2><strong>UKF\u53c2\u6570<\/strong><\/h2>\n<p>alpha = 0.001<\/p>\n<p>beta = 2<\/p>\n<p>kappa = 0<\/p>\n<p><\/code><\/pre>\n<\/p>\n<p><p><strong>\u8fd0\u884cUKF<\/strong><br \/>\u7ed3\u5408\u6240\u6709\u6b65\u9aa4\uff0c\u8fd0\u884cUKF\u4ee5\u6ee4\u9664\u566a\u58f0\u5e76\u4f30\u8ba1\u72b6\u6001\u3002<\/p>\n<\/p>\n<p><pre><code class=\"language-python\"># \u521d\u59cb\u5316UKF<\/p>\n<p>current_state_estimate = initial_state_estimate<\/p>\n<p>current_covariance = initial_covariance<\/p>\n<h2><strong>\u8fd0\u884cUKF<\/strong><\/h2>\n<p>for t in range(time_steps):<\/p>\n<p>    # \u751f\u6210sigma\u70b9<\/p>\n<p>    sigma_points, weights_mean, weights_covariance = generate_sigma_points(current_state_estimate, current_covariance, alpha, beta, kappa)<\/p>\n<p>    # \u9884\u6d4b\u6b65\u9aa4<\/p>\n<p>    predicted_state, predicted_covariance, predicted_sigma_points = predict(sigma_points, weights_mean, weights_covariance, process_noise_covariance)<\/p>\n<p>    # \u6a21\u62df\u6d4b\u91cf<\/p>\n<p>    measurement = np.array([np.sin(t), np.cos(t)])  # \u793a\u4f8b\u6d4b\u91cf<\/p>\n<p>    # \u66f4\u65b0\u6b65\u9aa4<\/p>\n<p>    current_state_estimate, current_covariance = update(predicted_state, predicted_covariance, predicted_sigma_points, measurement, measurement_noise_covariance)<\/p>\n<p>    print(f&quot;Time {t}: Estimated State: {current_state_estimate}&quot;)<\/p>\n<p><\/code><\/pre>\n<\/p>\n<p><p><strong>\u603b\u7ed3<\/strong><br \/>\u901a\u8fc7\u4ee5\u4e0a\u6b65\u9aa4\uff0c\u53ef\u4ee5\u5728Python\u4e2d\u6210\u529f\u5b9e\u73b0\u65e0\u8ff9\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u3002UKF\u662f\u4e00\u79cd\u5f3a\u5927\u7684\u6ee4\u6ce2\u6280\u672f\uff0c\u9002\u7528\u4e8e\u5904\u7406\u975e\u7ebf\u6027\u7cfb\u7edf\u4e2d\u7684\u72b6\u6001\u4f30\u8ba1\u95ee\u9898\u3002\u901a\u8fc7\u7ec6\u81f4\u5730\u5b9a\u4e49\u72b6\u6001\u6a21\u578b\u3001\u751f\u6210sigma\u70b9\u3001\u5b9e\u65bd\u9884\u6d4b\u548c\u66f4\u65b0\u6b65\u9aa4\uff0cUKF\u80fd\u591f\u6709\u6548\u5730\u6ee4\u9664\u566a\u58f0\u5e76\u63d0\u4f9b\u7cbe\u51c6\u7684\u72b6\u6001\u4f30\u8ba1\u3002\u5728\u5b9e\u9645\u5e94\u7528\u4e2d\uff0c\u53ef\u80fd\u9700\u8981\u6839\u636e\u5177\u4f53\u7684\u7cfb\u7edf\u548c\u6d4b\u91cf\u7279\u6027\u5bf9UKF\u8fdb\u884c\u8c03\u6574\u548c\u4f18\u5316\u3002<\/p>\n<\/p>\n<h2><strong>\u76f8\u5173\u95ee\u7b54FAQs\uff1a<\/strong><\/h2>\n<p> <strong>\u5982\u4f55\u5728Python\u4e2d\u5b9e\u73b0UKF\uff08\u65e0\u8ff9\u5361\u5c14\u66fc\u6ee4\u6ce2\uff09\uff1f<\/strong><br \/>UKF\u662f\u4e00\u79cd\u7528\u4e8e\u975e\u7ebf\u6027\u72b6\u6001\u4f30\u8ba1\u7684\u6ee4\u6ce2\u5668\u3002\u8981\u5728Python\u4e2d\u5b9e\u73b0UKF\uff0c\u901a\u5e38\u9700\u8981\u4f7f\u7528NumPy\u548cSciPy\u5e93\u6765\u5904\u7406\u6570\u5b66\u8fd0\u7b97\u3002\u4f60\u53ef\u4ee5\u9009\u62e9\u81ea\u5df1\u4ece\u5934\u5b9e\u73b0\uff0c\u6216\u8005\u5229\u7528\u73b0\u6709\u7684\u5e93\uff0c\u5982<code>filterpy<\/code>\uff0c\u8fd9\u662f\u4e00\u4e2a\u5e38\u7528\u7684\u6ee4\u6ce2\u5668\u5e93\uff0c\u63d0\u4f9b\u4e86UKF\u7684\u5b9e\u73b0\u3002<\/p>\n<p><strong>UKF\u7684\u57fa\u672c\u6b65\u9aa4\u662f\u4ec0\u4e48\uff1f<\/strong><br \/>UKF\u7684\u5b9e\u73b0\u901a\u5e38\u5305\u62ec\u4ee5\u4e0b\u51e0\u4e2a\u6b65\u9aa4\uff1a\u5b9a\u4e49\u72b6\u6001\u8f6c\u79fb\u548c\u89c2\u6d4b\u6a21\u578b\uff0c\u521d\u59cb\u5316\u72b6\u6001\u548c\u534f\u65b9\u5dee\u77e9\u9635\uff0c\u751f\u6210sigma\u70b9\uff0c\u5bf9sigma\u70b9\u8fdb\u884c\u9884\u6d4b\u548c\u66f4\u65b0\uff0c\u6700\u540e\u5408\u5e76\u7ed3\u679c\u4ee5\u66f4\u65b0\u72b6\u6001\u548c\u534f\u65b9\u5dee\u3002\u7406\u89e3\u8fd9\u4e9b\u6b65\u9aa4\u6709\u52a9\u4e8e\u66f4\u597d\u5730\u5b9e\u73b0UKF\u3002<\/p>\n<p><strong>\u5728Python\u4e2d\u4f7f\u7528UKF\u65f6\u9700\u8981\u6ce8\u610f\u54ea\u4e9b\u4e8b\u9879\uff1f<\/strong><br \/>\u5b9e\u73b0UKF\u65f6\uff0c\u9700\u8981\u6ce8\u610f\u9009\u62e9\u5408\u9002\u7684\u53c2\u6570\uff0c\u5982\u8fc7\u7a0b\u566a\u58f0\u548c\u89c2\u6d4b\u566a\u58f0\u7684\u534f\u65b9\u5dee\u77e9\u9635\uff0c\u8fd9\u4e9b\u53c2\u6570\u4f1a\u5f71\u54cd\u6ee4\u6ce2\u5668\u7684\u6027\u80fd\u3002\u6b64\u5916\uff0c\u786e\u4fdd\u6a21\u578b\u7684\u975e\u7ebf\u6027\u7a0b\u5ea6\u9002\u5408\u4f7f\u7528UKF\uff0c\u907f\u514d\u5bf9\u7ebf\u6027\u7cfb\u7edf\u4f7f\u7528UKF\u800c\u5bfc\u81f4\u4e0d\u5fc5\u8981\u7684\u590d\u6742\u6027\u3002<\/p>\n<p><strong>\u5982\u4f55\u8c03\u8bd5\u548c\u4f18\u5316UKF\u7684\u6027\u80fd\uff1f<\/strong><br \/>\u8c03\u8bd5UKF\u65f6\uff0c\u53ef\u4ee5\u901a\u8fc7\u5bf9\u6bd4\u4e0d\u540c\u53c2\u6570\u7ec4\u5408\u7684\u6ee4\u6ce2\u7ed3\u679c\u6765\u4f18\u5316\u6027\u80fd\u3002\u53ef\u89c6\u5316\u9884\u6d4b\u548c\u89c2\u6d4b\u7ed3\u679c\u7684\u5bf9\u6bd4\u4e5f\u662f\u6709\u6548\u7684\u65b9\u6cd5\u3002\u6b64\u5916\uff0c\u76d1\u63a7\u72b6\u6001\u4f30\u8ba1\u7684\u6536\u655b\u6027\u548c\u7a33\u5b9a\u6027\uff0c\u786e\u4fdd\u6ee4\u6ce2\u5668\u80fd\u591f\u5904\u7406\u5b9e\u9645\u5e94\u7528\u4e2d\u7684\u566a\u58f0\u548c\u4e0d\u786e\u5b9a\u6027\u3002<\/p>\n","protected":false},"excerpt":{"rendered":"\u5728Python\u4e2d\u5b9e\u73b0\u65e0\u8ff9\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\uff08UKF\uff09\u7684\u6838\u5fc3\u5728\u4e8e\u7406\u89e3UKF\u7684\u57fa\u672c\u6982\u5ff5\u3001\u4f7f\u7528\u5408\u9002\u7684\u5e93\u4ee5\u53ca\u5904\u7406\u975e\u7ebf\u6027\u7cfb\u7edf\u3002 [&hellip;]","protected":false},"author":3,"featured_media":959001,"comment_status":"closed","ping_status":"","sticky":false,"template":"","format":"standard","meta":{"_acf_changed":false,"footnotes":""},"categories":[37],"tags":[],"acf":[],"_links":{"self":[{"href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/posts\/958994"}],"collection":[{"href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/users\/3"}],"replies":[{"embeddable":true,"href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/comments?post=958994"}],"version-history":[{"count":"1","href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/posts\/958994\/revisions"}],"predecessor-version":[{"id":959004,"href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/posts\/958994\/revisions\/959004"}],"wp:featuredmedia":[{"embeddable":true,"href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/media\/959001"}],"wp:attachment":[{"href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/media?parent=958994"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/categories?post=958994"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/docs.pingcode.com\/wp-json\/wp\/v2\/tags?post=958994"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}